/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "programengine.h"
#include "taskmanager.h"
#include "messagelog.h"
#include "mccontrollerthread.h"
#include <QDebug>
#include <QFile>
#include "robotcontrol.h"
#include "echoserver.h"
#include "commrobotiq.h"
#include "powermanager.h"
#include "programtexttransfer.h"
#include "ifinstructionlogic.h"
#include "abortinstructionlogic.h"
#include "callinstructionlogic.h"
#include "coordinateinstructionlogic.h"
#include "endinstructionlogic.h"
#include "errorinstructionlogic.h"
#include "forinstructionlogic.h"
#include "gotoinstructionlogic.h"
#include "gripperinstructionlogic.h"
#include "ioinstructionlogic.h"
#include "jumpinstructionlogic.h"
#include "labelinstructionlogic.h"
#include "messageinstructionlogic.h"
#include "motioninstructionlogic.h"
#include "overrideinstructionlogic.h"
#include "palinstructionlogic.h"
#include "pauseinstructionlogic.h"
#include "reginstructionlogic.h"
#include "rsrinstructionlogic.h"
#include "runinstructionlogic.h"
#include "selectinstructionlogic.h"
#include "timerinstructionlogic.h"
#include "ualminstructionlogic.h"
#include "waitinstructionlogic.h"
#include "movemagneticinstructionlogic.h"
#include "movefreeinstructionlogic.h"
#include "musicinstructionlogic.h"
#include "charginginstructionlogic.h"
#include "liftmechanisminstructionlogic.h"
#include "tcpstartinstructionlogic.h"
#include "tcpreadinstructionlogic.h"
#include "tcpwriteinstructionlogic.h"
#include "tcpmanager.h"
#include "loadmapinstructionlogic.h"
#include "checkbatteryinstructionlogic.h"
#include "setodominstructionlogic.h"
#include "waitcommandinstructionlogic.h"
#include "robotmoveinstructionlogic.h"
#include "rollerinstructionlogic.h"
#include "modbusstartinstructionlogic.h"
#include "modbusreadinstructionlogic.h"
#include "modbuswriteinstructionlogic.h"
#include "visionrunlogic.h"
#include "trackstartlogic.h"
#include "weldendinstructionlogic.h"
#include "weldstartinstructionlogic.h"
#include "weavestartinstructionlogic.h"
#include "trackweldtastinstructionlogic.h"
#include "trackweldendinstructionlogic.h"
#include "watchdiinstructionlogic.h"
#include "weaveinstructionlogic.h"
#include "lasercameraadapter.h"
#include "lasercamerainstructionlogic.h"
#include "lasercameratrajserver.h"

#include "fleetservice.h"

extern FleetService *fleetService;

extern PowerManager *p_powerManger;

ProgramEngine::ProgramEngine(int id, RobotIo *ioControllerIn, RegData *mainRegDataIn, RsrInfo *tmpRsrInfo, int robotEngineStartTypeIn,
                             McControllerThread *proMcControllerThreadIn, QObject *parent)
    :robotId(id),QObject(parent)
{
    regData=mainRegDataIn;
    rsrInfo=tmpRsrInfo;

    versionInfo.componentName="ProgramEngine";
    versionInfo.componentVersion="V1.0";
    versionInfo.publishDate="2018.10.27";


    quickStopFlag=false;
    isInRunningLoop=false;
    isProgramEngineInitialOk=false;
    ioController=ioControllerIn;
    programMcController=proMcControllerThreadIn;

//    programCountSize = 0;
    runningLineFloat = 0;
    moveType = ENUM_MOVE_ORDER_STEP;
    runModel =  ENUM_CONTROL_T1;
    oneStepEnableFlag=false;
    debugEnable=false;
    engineStatus = ENUM_COMMAND_RESET;
    pcFlagValue = 0.0;

    isErrorOccured=false;
    programSet=new ProgramSet;
    currentPointProList=new QVector <PointPro>;
    allPointProList=new QVector <QVector <PointPro> >;

//    pointData=NULL;
//    regData=NULL;
//    rsrInfo = NULL;
    setRunNumFlag = false;
    isMainThread = true;
//    currentPalletStackInfo.clear();
    pointData = programMcController->mcControllerInterpreter->pointData;
    initialRegister(robotEngineStartTypeIn);

    // 初始化程序语言翻译对象
    programLanguageTranslate = new ProgramLanguageTranslate();

    //初始化线程
    currentThread=new QThread();
    currentThread->start();
    while( !currentThread->isRunning() )
    {
        usleep(100);
    }
    currentThread->setPriority(QThread::TimeCriticalPriority);
//    qDebug()<< "new threadMotionEngine,currentThread->currentThreadId() "
//            << currentThread->currentThreadId();
    this->moveToThread(currentThread);

    //initial taskmanager
    taskIndex=0;
    taskManager = new TaskManager(robotId);
    ProgramEngineInfo tmpProgramEngine;
    tmpProgramEngine.fileName = currentProgramName;
    tmpProgramEngine.ProgramEnginePointer = this;
    tmpProgramEngine.runningThread = currentThread;
    tmpProgramEngine.state=E_TASK_ENGINE_RUNNING;
    taskManager->addNewProgramEngine(tmpProgramEngine);
    for(int i=1;i<D_MAX_TASK_LIMIT;i++)
    {
       taskManager->createNewProgramEngine(0,i,ioController,regData,rsrInfo);
    }


    initialInstruction();

    //线程锁,  数据读写
    pthread_mutexattr_init(&programAttr);
    pthread_mutexattr_setprotocol(&programAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&programMutex,&programAttr);

    pthread_mutexattr_init(&engineStatusMutexAttr);
    pthread_mutexattr_setprotocol(&engineStatusMutexAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&engineStatusMutex,&engineStatusMutexAttr);

    connect( this, SIGNAL(runNewTask_signal(int)), this, SLOT(programEngineTaskLoop(int) ));

    createThread();

}

ProgramEngine::ProgramEngine(int id, RobotIo *ioControllerIn, RegData *mainRegData,
                             RsrInfo *tmpRsrInfo, TaskManager *taskManagerIn
                             ,  QThread *threadIn, int thisTaskIndex, QObject *parent)
    :robotId(id),QObject(parent)
{
    versionInfo.componentName="ProgramEngine";
    versionInfo.componentVersion="V1.0";
    versionInfo.publishDate="2018.10.27";

    isInRunningLoop=false;
    isProgramEngineInitialOk=false;
    ioController=ioControllerIn;
    taskIndex=thisTaskIndex;

//    programCountSize = 0;
    quickStopFlag=false;
    runningLineFloat = 0;
    moveType = ENUM_MOVE_ORDER_STEP;
    runModel =  ENUM_CONTROL_T1;
    oneStepEnableFlag=false;
    debugEnable=false;
    engineStatus = ENUM_COMMAND_RUN;
    pcFlagValue = 0.0;

    isErrorOccured=false;
    programSet=new ProgramSet;
    currentPointProList=new QVector <PointPro>;
    allPointProList=new QVector <QVector <PointPro> >;

    pointData=NULL;
    regData=mainRegData;
    programMcController=NULL;
    ioController=ioControllerIn;
    rsrInfo = tmpRsrInfo;
    setRunNumFlag = false;
    isMainThread = false;
//    currentPalletStackInfo.clear();

    //initial taskmanager
    taskManager = taskManagerIn;

    //初始化线程
    currentThread =threadIn;
    currentThread->start();
    while( !currentThread->isRunning() )
    {
        usleep(100);
    }
    currentThread->setPriority(QThread::TimeCriticalPriority);
    qDebug()<< "new threadMotionEngine,currentThread->currentThreadId() "
            << currentThread->currentThreadId();
    this->moveToThread(currentThread);

    initialInstruction();

    //线程锁,  数据读写
    pthread_mutexattr_init(&programAttr);
    pthread_mutexattr_setprotocol(&programAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&programMutex,&programAttr);

    pthread_mutexattr_init(&engineStatusMutexAttr);
    pthread_mutexattr_setprotocol(&engineStatusMutexAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&engineStatusMutex,&engineStatusMutexAttr);

}

ProgramEngine::~ProgramEngine()
{
//    threadStop();

//    if(NULL != currentThread)
//    {
//        delete currentThread;
//        currentThread = NULL;
//    }
    if(true==isMainThread)
    {
        if(NULL != regData)
        {
            delete regData;
            regData = NULL;
        }
        if(NULL != rsrInfo)
        {
            delete rsrInfo;
            rsrInfo = NULL;
        }
        if(NULL != taskManager)
        {
            delete taskManager;
            taskManager = NULL;
        }
    }

    deleteInstruction();

    
}

int ProgramEngine::startLaserScanBuffer()
{
    return lasercameraTrajServer->startBuffer();
}

int ProgramEngine::stopLaserScanBuffer()
{
    return lasercameraTrajServer->stopBuffer();
}

int ProgramEngine::getLaserScanTraj(QVector<PointPro > refOri,QVector<PointPro > &keyPointsOut)
{
    return lasercameraTrajServer->getTraj(refOri,keyPointsOut);
}

int ProgramEngine::get_All_IO_NamesInfo(IOMapNamesInfo &nameOut)
{
    return ioController->get_All_IO_NamesInfo(nameOut);
}

int ProgramEngine::set_IO_NameInfo(int IOValueTypeIn, int logicValueIndexIn, string nameIn, int &returnFlag)
{
    return ioController->set_IO_NameInfo(IOValueTypeIn,logicValueIndexIn,nameIn,returnFlag);
}

int ProgramEngine::getPalletConditionInfo(std::vector<PalletMiddleInfo> &infoOut)
{
    return palInstructionLogic->getPalletConditionInfo(infoOut);
}

int ProgramEngine::setPalletConditionInfo(StackInfo infoIn)
{
    return palInstructionLogic->setPalletConditionInfo(infoIn);
}

int ProgramEngine::getWeaveMotionConditionInfo(std::vector<WeaveMotionCondition> &infoOut)
{
    return weaveLogic->getWeaveMotionConditionInfo(infoOut);
}

int ProgramEngine::setWeaveMotionConditionInfo(WeaveMotionCondition infoIn)
{
    return weaveLogic->setWeaveMotionConditionInfo(infoIn);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::fetchWeldPoint(PointPro &tcpPositionOut,int waitCountIn)
{
    return laserCameraInstructionLogic->fetchWeldPoint(tcpPositionOut,waitCountIn);
}

bool ProgramEngine::getLaserCameraTrackStatus()
{
    return laserCameraInstructionLogic->getLaserCameraTrackStatus();
}

int ProgramEngine::getWeldDebugStatusInfo(WeldCraftStatusInfo &infoOut)
{
    return weldStartLogic->getWeldDebugStatusInfo(infoOut);
}

int ProgramEngine::weldDebugOperation(E_WELD_TEST typeIn, bool control)
{
    return weldStartLogic->weldDebugOperation(typeIn,control);
}

int ProgramEngine::setWeldCollisionCheck(bool isCheck)
{
    return weldStartLogic->setWeldCollisionCheck(isCheck);
}

E_ROLLER_STATUS ProgramEngine::getRollerMechanismStatus()
{
    return rollerInstructionLogic->getRollerMechanismStatus();
}

int ProgramEngine::setRollerMechanismConfig(RollerMechanismConfig configIn)
{
    return rollerInstructionLogic->setRollerMechanismConfig(configIn);
}

int ProgramEngine::getRollerMechanismConfig(RollerMechanismConfig &configOut)
{
    return rollerInstructionLogic->getRollerMechanismConfig(configOut);
}

E_LIFT_STATUS ProgramEngine::getLiftMechanismStatus()
{
    return liftMechanismInstructionLogic->getLiftMechanismStatus();
}

int ProgramEngine::setLiftMechanismConfig(LiftMechanismConfig configIn)
{
    return liftMechanismInstructionLogic->setLiftMechanismConfig(configIn);
}

int ProgramEngine::getLiftMechanismConfig(LiftMechanismConfig &configOut)
{
    return liftMechanismInstructionLogic->getLiftMechanismConfig(configOut);
}

bool ProgramEngine::getQuickStopFlag()
{
    return quickStopFlag;
}

int ProgramEngine::setWeldVirtualMode(bool isVirtualControlIn)
{
    return weldStartLogic->setVirtualMode(isVirtualControlIn);
}

bool ProgramEngine::getWeldIsVirtualStatus()
{
    return weldStartLogic->getWeldIsVirtualStatus();
}

int ProgramEngine::setRunningNumber_virtualLineControl(float numberIn)
{
    if(isMainThread)
    {
        return programMcController->mcControllerInterpreter->setRunningNumber_virtualLineControl(numberIn);
    }
    return 0;
    
}

int ProgramEngine::setRunningNumber_robotArmControl(float numberIn)
{
    if(isMainThread)
    {
        motionInstructionLogic->reset();
        return programMcController->mcControllerInterpreter->setRunningNumber_robotArmControl(numberIn);
    }
    return 0;
}

int ProgramEngine::getLaserCameraBufferMoveConfigParameter(BufferMoveConfig &paraOut)
{
    return lasercameraTrajServer->getLaserCameraBufferMoveConfigParameter(paraOut);
}

int ProgramEngine::setLaserCameraBufferMoveConfigParameter(BufferMoveConfig paraIn)
{
    return lasercameraTrajServer->writeConfigFile(paraIn);
}

int ProgramEngine::getLaserTrackConfigParameter(LaserTrackConfig &paraOut)
{
    return motionInstructionLogic->getLaserTrackConfigParameter(paraOut);
}

int ProgramEngine::setLaserTrackConfigParameter(LaserTrackConfig paraIn)
{
    return motionInstructionLogic->setLaserTrackConfigParameter(paraIn);
}

int ProgramEngine::getLaserCameraConfigParameter(LaserCameraConfigParameter &infoOut)
{
    return laserCameraAdapter->getLaserCameraConfigParameter(infoOut);
}

int ProgramEngine::getLaserCameraStatus(LaserCameraStatusInfo &infoOut)
{
    //工具号必须和标定时工具号一致，而且标定后，工具参数不得改变。现在记录的是相机和工具之间的偏移。
    if(1==laserCameraAdapter->getLaserCameraStatus(infoOut))
    {
//        PointPro tcpPosition,pointOut;
//        programMcController->mcControllerInterpreter->getCurrentCoordinatePoint_Feedback(tcpPosition);
//        programMcController->mcControllerInterpreter->getWeldPositionByCameraTransfer(
//                    tcpPosition,infoOut.real_x,infoOut.real_z,0,pointOut);
        programMcController->mcControllerInterpreter->getCameraPointInBase(infoOut.world_x,infoOut.world_y,infoOut.world_z);
//        if(6==pointOut.positionValue.size())
//        {
//            infoOut.world_x=pointOut.positionValue[0];
//            infoOut.world_y=pointOut.positionValue[1];
//            infoOut.world_z=pointOut.positionValue[2];
//        }
        return 1;
    }
    return -1;
}

int ProgramEngine::setLaserCameraConfigParameter(LaserCameraConfigParameter parameter)
{
    return laserCameraAdapter->setLaserCameraConfigParameter(parameter);
}

int ProgramEngine::reconnectLaserCamera()
{
    return laserCameraAdapter->reconnectLaserCamera();
}

int ProgramEngine::setLaserCameraWork(bool isWork)
{
    return laserCameraAdapter->setLaserCameraWork(isWork);
}

int ProgramEngine::setLaserCameraJobId(int jobIdIn)
{
    return laserCameraAdapter->setLaserCameraJobId(jobIdIn);
}

int ProgramEngine::setLaserCameraShowLaser(bool isShow)
{
    return laserCameraAdapter->setLaserCameraShowLaser(isShow);
}

int ProgramEngine::getWeldSystemInfo(struct WeldSystemInfo &weldSystemInfo){
    if(isMainThread)
    {
        return weldStartLogic->getWeldSystemInfo(weldSystemInfo);
    }
    return -1;
}
int ProgramEngine::changeWeldSystemInfo(struct WeldSystemInfo configIn){
    if(isMainThread)
    {
        return weldStartLogic->changeWeldSystemInfo(configIn);
    }
    return -1;
}

int ProgramEngine::getWeldProperty(std::vector<WeldProperty> &configListOut)
{
    if(isMainThread)
    {
        return weldStartLogic->getWeldProperty(configListOut);
    }
    return -1;
}

int ProgramEngine::chanegWeldPropertyConfig(struct WeldProperty &weldProperty)
{
    if(isMainThread)
    {
        return weldStartLogic->changeWeldPropertConfig(weldProperty);
    }
    return -1;
}

int ProgramEngine::getWeldCondition(std::vector<WeldCondition> &configListOut){
    if(isMainThread)
    {
        return weldStartLogic->getWeldCondition(configListOut);
    }
    return -1;
}

int ProgramEngine::chanegWeldConditionConfig(struct WeldCondition &weldCondition){
    if(isMainThread)
    {
        return weldStartLogic->changeWeldConditionConfig(weldCondition);
    }
    return -1;
}


int ProgramEngine::moveToNavgationPoint(MovingToNavagationParameter parameterIn, int &returnFlag)
{
    if(isMainThread)
    {
        return moveFreeInstructionLogic->moveToNavgationPoint(parameterIn,returnFlag);
    }
    return -1;
}

int ProgramEngine::addMonitorDi_fleet(bool isOr, QVector<int> diIndex, QVector<int> diValue)
{
    if(isMainThread)
    {
        return motionInstructionLogic->addMonitorDi_fleet(isOr,diIndex,diValue);
    }
    return 0;
}

int ProgramEngine::addCircle( int idIn, std::vector<double> posIn, std::vector<double> centerIn,
                              std::vector<double> normalIn, double vel, int turnsIn, int velTypeIn)
{
    if(isMainThread)
    {
        return motionInstructionLogic->addCircle(idIn,posIn,centerIn,normalIn,vel,turnsIn,velTypeIn);
    }
    return 0;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::addProgramMoveCommand_hmi(vector<MoveParameter> &tmpCommandVectorIn)
{
    if(isMainThread)
    {
        return motionInstructionLogic->addProgramMoveCommand_hmi(tmpCommandVectorIn);
    }
    return E_PROGRAM_LOOP_RESULT_ERROR;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::moveRobotArm_run(const QVector<int> &lineList,
                                                      const QVector<RobotArmMoveParameter> &pathListParaIn, int isLastPointMove)
{
    if(isMainThread)
    {
        return motionInstructionLogic->moveRobotArm_run(lineList,pathListParaIn,isLastPointMove);
    }
    return E_PROGRAM_LOOP_RESULT_ERROR;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::moveMultiPoint_run(const QVector<int> &lineList,
                              const QVector<LaserPathParameter> &laserPathListParaIn, int isLastPointMove)
{
    if(isMainThread)
    {
        return moveFreeInstructionLogic->moveToMultiPointByPathStyle_run(lineList,laserPathListParaIn,isLastPointMove);
    }
    return E_PROGRAM_LOOP_RESULT_ERROR;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::moveMagnetic_run(const MaganeticPathParameter paraIn)
{
    if(isMainThread)
    {
        return moveMagneticInstructionLogic->moveMagnetic_run(paraIn);
    }
    return E_PROGRAM_LOOP_RESULT_ERROR;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::roller_run(const RollerParameter &paraIn)
{
    if(isMainThread)
    {
        return rollerInstructionLogic->roller_run(paraIn);
    }
    return E_PROGRAM_LOOP_RESULT_ERROR;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::charging_run(const ChargeParameter &paraIn)
{
    if(isMainThread)
    {
        return chargingInstructionLogic->charging_run(paraIn);
    }
    return E_PROGRAM_LOOP_RESULT_ERROR;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::liftMechanism_run(const LiftParameter &paraIn)
{
    if(isMainThread)
    {
        return liftMechanismInstructionLogic->liftMechanism_run(paraIn);
    }
    return E_PROGRAM_LOOP_RESULT_ERROR;
}

int ProgramEngine::playMusic(QString musicFileName, int musicPlayTimes, int musicVolume)
{

    return musicInstructionLogic->playMusic(musicFileName,musicPlayTimes,musicVolume);
}

int ProgramEngine::playSystemMusic(QString nameIn, int musicPlayTimes)
{
    return musicInstructionLogic->playSystemMusic(nameIn,musicPlayTimes);
}

int ProgramEngine::playBarriorMusic(double barrierRatioIn)
{
    return musicInstructionLogic->playBarriorMusic(barrierRatioIn);
}

int ProgramEngine::playErrorMusic()
{
    return musicInstructionLogic->playErrorMusic();
}

int ProgramEngine::restoreNormalMusic()
{
    return musicInstructionLogic->restoreNormalMusic();
}

int ProgramEngine::setPlRegister(int palletNumber, PlRegStruct plReg, int &returnFlag)
{

    regData->setPlRegValue(palletNumber,plReg);
    programMcController->mcControllerInterpreter->setPalletPlFlag(palletNumber);
    returnFlag = 0;

    return 1;
}

int ProgramEngine::addCommand(UserCommandData commandIn)
{
    return waitCommandInstructionLogic->addCommand(commandIn);
}

int ProgramEngine::getDpoint(string fileName, int pointIndex, DPoint &pointOut)
{
    ProgramTextTransfer programFransfer(robotId);
    return programFransfer.getDpoint(fileName,pointIndex,pointOut);
}

int ProgramEngine::recordDpoint(string fileName, DPoint pointIn)
{
    ProgramTextTransfer programFransfer(robotId);
    return programFransfer.recordDpoint(fileName,pointIn);
}

int ProgramEngine::get_robot_running_program_files_info(std::vector<string> &fileNames, std::vector<int> &runningLines)
{
    CurrentRunFile tmpRunRobotFile;
    getCurrentRobotRunFile(tmpRunRobotFile);
    fileNames=tmpRunRobotFile.runingFileName;
    runningLines=tmpRunRobotFile.runningFileLine;
    return 1;
}

int ProgramEngine::get_running_program_file_strings(int languageId, QString fileName,
                                                    QVector<QString> &programStrings)
{
    qDebug()<<"ProgramEngine::get_running_program_file_strings begin";

    if(1!=programSet->getProgramString_byName(fileName,programStrings))
    {
        return -1;
    }

    QStringList tmpStrList;
    // 翻译1个程序文件
        for(int i = 0; i < programStrings.size(); i++)
        {
            tmpStrList << programStrings.at(i);
        }

        // 程序语言翻译
        tmpStrList = programLanguageTranslate->translateProgramCmdList(languageId,tmpStrList);
        programStrings.clear();
        for(int i= 0; i < tmpStrList.size(); i++)
        {
            programStrings.append(tmpStrList.at(i));
        }



        return 1;
}

int ProgramEngine::get_running_program_file_strings_task(int languageId, QString fileName,
                                                         QVector<QString> &programStrings)
{
    return taskManager->get_running_program_file_strings_task(languageId,fileName,programStrings);
}


int ProgramEngine::getProgramRunningInfo(int languageId,std::vector<std::string> &fileNames, std::vector<int> &runningLines,
                                         QVector<QVector<QString> > &programStrings)
{
    qDebug()<<"ProgramEngine::getProgramRunningInfo begin";
//    //todo 根据　languageId返回中文或英文。封装一个翻译类ProgramLanguageTranslate 去处理翻译，使hmi　和　fundation 可以复用。
//    programSet->getProgramString(programStrings);

//    QStringList tmpStrList;
//    // 翻译多个程序文件
////    qDebug()<<"翻以前　programStrings.size()="<< programStrings.size();
//    for(int j =0; j < programStrings.size(); j++)
//    {
//        for(int i = 0; i < programStrings[j].size(); i++)
//        {
//            tmpStrList << programStrings[j].at(i);
//        }
////        qDebug()<<"j="<<j<<"翻译前　tmpStrList＝"<< tmpStrList.size();
//        // 程序语言翻译
//        tmpStrList = programLanguageTranslate->translateProgramCmdList(languageId,tmpStrList);
////        qDebug()<<"翻译后　tmpStrList="<< tmpStrList.size();
//        programStrings[j].clear();
//        for(int i= 0; i < tmpStrList.size(); i++)
//        {
//            programStrings[j].append(tmpStrList.at(i));
//        }

//        tmpStrList.clear();
//    }


    CurrentRunFile tmpRunRobotFile;
    getCurrentRobotRunFile(tmpRunRobotFile);
    fileNames=tmpRunRobotFile.runingFileName;
    runningLines=tmpRunRobotFile.runningFileLine;

    for(int i=0;i<fileNames.size();i++)
    {
        QVector<QString> tmpProgramStrings;
        get_running_program_file_strings_task(languageId,QString::fromStdString(fileNames[i])
                                              ,tmpProgramStrings);
        programStrings.append(tmpProgramStrings);
    }

//    qDebug()<<"翻译后　programStrings.size="<<programStrings.size();
//    qDebug()<<"ProgramEngine::getProgramRunningInfo end";
    return 1;
}

int ProgramEngine::setDefaultProgram(QString defaultProgramNameIn)
{
    defaultProgramName=defaultProgramNameIn;
    return 1;
}

bool ProgramEngine::isInRunningLoopStatus()
{
    return isInRunningLoop;

}

bool ProgramEngine::get_mamual_run_flag()
{
    return oneStepEnableFlag;
}

bool ProgramEngine::set_mamual_run_flag(bool value)
{
    oneStepEnableFlag=value;
    return true;
}

int ProgramEngine::getMoveType()
{
    return moveType;
}


bool ProgramEngine::isMainThreadTrue()
{
    return isMainThread;
}

//QString ProgramEngine::getCurrentFile()
//{
//    return currentFilePath;
//}

//int ProgramEngine::setProgramCountNum(int num)
//{
//    programCountNum=num;
//    return 1;
//}

//int ProgramEngine::getProgramSetSize()
//{
//    return programSet->size();
//}

float ProgramEngine::getRunningLineFloat()
{
    return runningLineFloat;
}

int ProgramEngine::setRunningLineFloat(float lineIn)
{
    runningLineFloat=lineIn;
    return 1;
}

int ProgramEngine::getCmdStrList(QVector<QString> &listStrOut)
{
    cmdStrListMutex.lock();
    listStrOut=cmdStrList;
    cmdStrListMutex.unlock();
    return 1;
}

QString ProgramEngine::getCurrentProgramName()
{
    return currentProgramName;
}

int ProgramEngine::getEngineStatue()
{
    return engineStatus;
}

int ProgramEngine::setMotionStage(E_PROGRAM_MOTION_STAGE stage)
{
    engineDebugInfo.motionStage=stage;
}

bool ProgramEngine::getSetRunNumFlag()
{
    return setRunNumFlag;
}

bool ProgramEngine::writeSetRunNumFlag(bool value)
{
    setRunNumFlag=value;
    return true;
}

int ProgramEngine::setMotionInstructionAdded(int number)
{
    engineDebugInfo.motionInstrutionAdd=number;
    return 1;
}

int ProgramEngine::setCurrentProgramName(QString fileName)
{
    taskManager->modifyProgramEningeFileName(taskIndex,fileName);
    currentProgramName=fileName;
    return 1;
}

int ProgramEngine::getDebugInfo(ProgramEngineDebugInfo &infoOut)
{
    infoOut=engineDebugInfo;
    infoOut.programCountNum=0;//mutex problem ,not use
    infoOut.runningLineFloat=runningLineFloat;
    infoOut.programInstructionType=programInstructionType;
    infoOut.engineStatus=engineStatus;

    return 1;
}

int ProgramEngine::getDebugInfoList(std::vector<ProgramEngineDebugInfo> &infoListOut)
{
//    if(trylock())//导致死锁，与TaskManager::getCurrentRobotRunFile
//    {
        infoListOut.clear();
        ProgramEngineDebugInfo tmpInfo;
        for(int i=0;i<taskManager->getCurrentTaskSize();i++)
        {
            taskManager->getProgramEngineDebugInfo(i,tmpInfo);
            infoListOut.push_back(tmpInfo);
        }
//        unlock();

//    }
    return 1;
}

int ProgramEngine::getTaskInfo(TaskManagerInfo& taskManagerInfo)
{
    taskManager->getTaskInfo(taskManagerInfo);
    return 1;
}

EN_CONTROL_TYPE ProgramEngine::getRunModel()
{
    return runModel;
}

int ProgramEngine::readVersion(std::vector <ComponentVersion> &componentVersionList)
{
    componentVersionList.push_back(versionInfo);
    programMcController->mcControllerInterpreter->readVersion(componentVersionList);
    pointData->readVersion(componentVersionList);
    regData->readVersion(componentVersionList);
    return 1;
}

//int ProgramEngine::engineThreadDestory()
//{
//    lock();
//    int childMount = taskManager->programEngineList.size();
//    //0不销毁
//    for(int i = 1; i <childMount;i++)
//    {
//        qDebug()<<"ProgramEngine::engineThreadDestory()"
//        <<"===,QThread::currentThreadId()="<<QThread::currentThreadId()
//        <<"===,currentThread->currentThreadId()="<<currentThread->currentThreadId()
//        <<"\n\n";

//        qDebug()<<"to delete thread"
//        <<"===,taskManager->programEngineList[i].ProgramEnginePointer->currentThread="
//        <<taskManager->programEngineList[i].ProgramEnginePointer->currentThread->currentThreadId()
//        <<"===,taskManager->programEngineList[i].runningThread->currentThreadId()="<<currentThread->currentThreadId()
//        <<taskManager->programEngineList[i].runningThread->currentThreadId()
//        <<"\n\n";
//        if(NULL != taskManager->programEngineList[i].ProgramEnginePointer)
//        {
//            while(true==taskManager->programEngineList[i].ProgramEnginePointer
//                  ->isInRunningLoopStatus())
//            {
//                usleep(100000);
//            }
//            delete taskManager->programEngineList[i].ProgramEnginePointer;
//            taskManager->programEngineList[i].ProgramEnginePointer = NULL;
//            qDebug()<<"delete sucess,taskIndex="<<i;
//        }

//    }
//    unlock();
//    resetTaskManager();
//    return 1;
//}

//bool ProgramEngine::threadStop()
//{
//    engineStatus = ENUM_COMMAND_RESET;

//    if(NULL != currentThread)
//    {
//        currentThread->isFinished();
//        currentThread->quit();
//        if(!currentThread->wait())
//        {
//            qDebug()<<"Programthread quit failed"
//            <<"===,QThread::currentThreadId()="<<QThread::currentThreadId()
//            <<"===,currentThread->currentThreadId()="<<currentThread->currentThreadId()
//            <<"\n\n";

//            return false;
//        }
//        qDebug()<<"Programthread quit sucess"
//        <<"===,QThread::currentThreadId()="<<QThread::currentThreadId()
//        <<"===,currentThread->currentThreadId()="<<currentThread->currentThreadId()
//        <<"\n\n";
//    }
//    return true;
//}

int ProgramEngine::initialInstruction()
{
    ifInstructionLogic=new IfInstructionLogic(robotId,programSet,ioController,this);

     abortInstructionLogic=new AbortInstructionLogic(robotId,programSet,ioController,this);
     callInstructionLogic=new CallInstructionLogic(robotId,programSet,ioController
                       ,this,pointData,currentPointProList,
                                                   allPointProList
                                                   ,programMcController);
     coordinateInstructionLogic=new CoordinateInstructionLogic(robotId,ioController
                                                               ,this,programMcController,regData);
     endInstructionLogic=new EndInstructionLogic(robotId,programSet,ioController,this);
     errorInstructionLogic=new ErrorInstructionLogic(robotId,programSet,ioController,this);
     forInstructionLogic=new ForInstructionLogic(robotId,programSet,ioController,this);
     goToInstructionLogic=new GoToInstructionLogic(robotId,programSet,ioController,this);
     gripperInstructionLogic=new GripperInstructionLogic(robotId,programSet,ioController,this);
     ioInstructionLogic=new IoInstructionLogic(robotId,programSet,ioController,this);
     jumpInstructionLogic=new JumpInstructionLogic(robotId,programSet,ioController,this);
     labelInstructionLogic=new LabelInstructionLogic(robotId,programSet,ioController,this);
     messageInstructionLogic=new MessageInstructionLogic(robotId,programSet,ioController,this);

     overrideInstructionLogic=new OverrideInstructionLogic(robotId,programSet,ioController
                                                           ,this,programMcController);

     pauseInstructionLogic=new PauseInstructionLogic(robotId,programSet,ioController,this);
     regInstructionLogic=new RegInstructionLogic(robotId,programSet,ioController
                                                 ,this,
                                                 regData,programMcController);
     rsrInstructionLogic=new RsrInstructionLogic(robotId,programSet,ioController
                                                 ,this,rsrInfo);
     runInstructionLogic=new RunInstructionLogic(robotId,programSet,ioController,this);
     selectInstructionLogic=new SelectInstructionLogic(robotId,programSet,ioController,this);
     timerInstructionLogic=new TimerInstructionLogic(robotId,programSet,ioController
                                                     ,this,regData);
     ualmInstructionLogic=new UalmInstructionLogic(robotId,programSet,ioController,this);
     waitInstructionLogic=new WaitInstructionLogic(robotId,programSet,ioController,this);

     musicInstructionLogic = new MusicInstructionLogic(robotId, programSet, this);

     if(true==isMainThread)
     {
         QString tmpPalFilePath=D_TCRCONTROLLER_DATA_PATH;
         tmpPalFilePath = tmpPalFilePath+ROBOT_COMMON_NAME + QString::number(robotId)
                 + "/config/InstructionConfig/palletConfig.json";

         QString tmpCameraConfigFilePath=D_TCRCONTROLLER_DATA_PATH;
         tmpCameraConfigFilePath = tmpCameraConfigFilePath+ROBOT_COMMON_NAME + QString::number(robotId)
                 + "/config/laserCameraConfig.xml";

         palInstructionLogic=new PalInstructionLogic(robotId,regData,ioController
                                                     ,this,programMcController,tmpPalFilePath);

         motionInstructionLogic=new MotionInstructionLogic(tmpCameraConfigFilePath,robotId,programSet,ioController
                                                           ,this,programMcController,currentPointProList,
                                                           &regPlList);
         moveMagneticInstructionLogic=new MoveMagneticInstructionLogic(robotId,programSet
                                                                       ,this,programMcController);
         moveFreeInstructionLogic=new MoveFreeInstructionLogic(robotId,programSet
                                  ,this,programMcController,&currentVehicleDPointList);
         chargingInstructionLogic = new ChargingInstructionLogic(robotId, programSet, this,
                                                                 ioController,moveFreeInstructionLogic);
         liftMechanismInstructionLogic = new LiftMechanismInstructionLogic(ioController, robotId, programSet,
                                                                           this,programMcController);
         rollerInstructionLogic = new RollerInstructionLogic(ioController, robotId, programSet,
                                                             this,programMcController,liftMechanismInstructionLogic);
         visionRunLogic=new VisionRunLogic(robotId, this,programMcController);
         trackStartLogic=new TrackStartLogic(robotId,this,programMcController);
         QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
         tmpFilePath = tmpFilePath+ROBOT_COMMON_NAME + QString::number(robotId)
                 + "/config/InstructionConfig/weldConfig.xml";
         QString tmpWaveFilePath=D_TCRCONTROLLER_DATA_PATH;
         tmpWaveFilePath = tmpWaveFilePath+ROBOT_COMMON_NAME + QString::number(robotId)
                 + "/config/InstructionConfig/weaveConfig.xml";

         weldStartLogic=new WeldStartInstructionLogic(tmpFilePath,robotId,programSet,ioController,this);
         #ifndef D_USE_ROS
         weaveLogic=new WeaveInstructionLogic(tmpWaveFilePath,robotId,programSet,ioController,this,programMcController);
         trackWeldTastLogic=new TrackWeldTastInstructionLogic(tmpFilePath,robotId,programSet,ioController,this);
         trackWeldEndLogic=new TrackWeldEndInstructionLogic(tmpFilePath,robotId,programSet,ioController,this);
         watchDILogic=new WatchDIInstructionLogic(tmpFilePath,robotId,programSet,ioController,this);
         #endif

         laserCameraAdapter=new LaserCameraAdapter(tmpCameraConfigFilePath,robotId,programMcController);

         laserCameraInstructionLogic=new LaserCameraInstructionLogic(robotId,programSet,ioController
                                               ,this,programMcController,regData);
         lasercameraTrajServer=new LaserCameraTrajServer(tmpCameraConfigFilePath,robotId,programMcController);
}


     
     tcpStartInstructionLogic = new TcpStartInstructionLogic(robotId, programSet, this);
     tcpReadInstructionLogic = new TcpReadInstructionLogic(robotId, programSet, this, regData);
     tcpWriteInstructionLogic = new TcpWriteInstructionLogic(robotId, programSet, this, regData);

     loadMapInstructionLogic = new LoadMapInstructionLogic(robotId, programSet, this, programMcController);
     setOdomInstructionLogic = new SetOdomInstructionLogic(robotId, programSet, this, programMcController);
     
     checkBatteryInstructionLogic = new CheckBatteryInstructionLogic(robotId, programSet, ioController,this);
     waitCommandInstructionLogic = new WaitCommandInstructionLogic(robotId, programSet,regData,this);

     robotMoveInstructionLogic = new RobotMoveInstructionLogic(robotId, programSet,this, regData);

     // MODBUS
     modbusStartInstructionLogic = new ModbusStartInstructionLogic(robotId, programSet,this, regData);
     modbusReadInstructionLogic = new ModbusReadInstructionLogic(robotId, programSet,this, regData);
     modbusWriteInstructionLogic = new ModbusWriteInstructionLogic(robotId, programSet,this, regData);

     return 1;
}

int ProgramEngine::deleteInstruction()
{
     delete ifInstructionLogic;
     delete abortInstructionLogic;
     delete callInstructionLogic;
     delete coordinateInstructionLogic;
     delete endInstructionLogic;
     delete errorInstructionLogic;
     delete forInstructionLogic;
     delete goToInstructionLogic;
     delete gripperInstructionLogic;
     delete ioInstructionLogic;
     delete jumpInstructionLogic;
     delete labelInstructionLogic;
     delete messageInstructionLogic;

     delete overrideInstructionLogic;
     delete palInstructionLogic;
     delete pauseInstructionLogic;
     delete regInstructionLogic;
     delete rsrInstructionLogic;
     delete runInstructionLogic;
     delete selectInstructionLogic;
     delete timerInstructionLogic;
     delete ualmInstructionLogic;
     delete waitInstructionLogic;

     delete musicInstructionLogic;

     delete tcpStartInstructionLogic;
     delete tcpWriteInstructionLogic;
     delete tcpReadInstructionLogic;
     delete loadMapInstructionLogic;
     delete setOdomInstructionLogic;
     delete robotMoveInstructionLogic;
     delete modbusStartInstructionLogic;
     delete modbusReadInstructionLogic;
     delete modbusWriteInstructionLogic;

    if(true==isMainThread)
    {
        delete motionInstructionLogic;
        delete rollerInstructionLogic;
        delete moveMagneticInstructionLogic;
        delete moveFreeInstructionLogic;
        delete chargingInstructionLogic;
        delete liftMechanismInstructionLogic;
    }

     return 1;
}


#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void* ProgramEngine::threadRun(void *)
{
    printf("ProgramEngine theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 40;
    sched_setscheduler(0,SCHED_FIFO,&param);
    qDebug()<<"************************ProgramEngine pid="<<gettid();
    usleep(659000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);


    while(  false==p_powerManger->isEthercatDeviceInitialOk())
    {
        usleep(50000);
    }
    while(1)
    {
        usleep(20000);

        weldStartLogic->weldWorkingMonitor();
    }
    printf("ProgramEngine thread quit!!!");
}

void* ProgramEngine::threadTask( void* classPtr )
{
    qDebug()<<"ProgramEngine thread run-------------------------";
    return ((ProgramEngine*)classPtr)->threadRun(NULL);
}

void ProgramEngine::createThread()
{
    pthread_t threadId;
    int ret=pthread_create( &threadId, NULL, &ProgramEngine::threadTask,this);
    qDebug()<<"ProgramEngine createThread-------------------------";
}


void ProgramEngine::realTimeSleep(long long umicoroSecond)
{
    // 处理qt事件
    QCoreApplication::processEvents();

    struct timespec time;
    clock_gettime(CLOCK_REALTIME,&time);
    time.tv_nsec += umicoroSecond * 1000;
    while(time.tv_nsec >= NSEC_PER_SEC)
    {
        time.tv_nsec -= NSEC_PER_SEC;
        ++time.tv_sec;
    }
    clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&time,NULL);

}

void ProgramEngine::initialRegister(int robotEngineStartType)
{
    startType = robotEngineStartType;

//    if(NULL == regData)
//    {
//      regData = new RegData;
//      regData->Init(startType);
//    }
    if( NULL != programMcController->mcControllerInterpreter )
    {
        pointData = programMcController->mcControllerInterpreter->pointData;
//        programMcController->mcControllerInterpreter->addMcInterpreterRegData(regData);
    }
    else
    {
        qDebug()<< "22  if(  NULL != programMcController->mcControllerInterpreter )";
    }

//    if(NULL == rsrInfo)
//    {
//        rsrInfo = new RsrInfo(robotId);
//    }

}


//从文件中更新用户坐标系数据
int ProgramEngine::updateUserCoordinateData(QVector<UserCoordinateInfo> userCoordinateList)
{
    return 0;
}

//从文件中更新工具坐标系数据
int ProgramEngine::updateToolCoordinateData(QVector<ToolCoordinateInfo> toolCoordinateList)
{
    return 0;
}

int ProgramEngine::setErrorMsgModel()
{
//    runModel = ENUM_CONTROL_T1;

//    if(isMainThread)
//    {
//        programMcController->mcControllerInterpreter->controlTypeSwitch(ENUM_CONTROL_T1);
//    }
//    if((ENUM_COMMAND_RESET != engineStatus))
//    {
////        qDebug()<<__FUNCTION__<<"====="<<__LINE__<<"\n\n\n\n\n";
//        resetProgram();
//    }

    return 0;
}

//int ProgramEngine::setTimerReg(int timerNum, int processType)
//{
////    initProgramSet();

//   if(TIMER_PROCESS_START == processType)
//   {
//        if((TIMER_PROCESS_START == programSet->timerProcessType[timerNum])
//                ||(TIMER_PROCESS_REG == programSet->timerProcessType[timerNum]))
//        {

//        }
//        else
//        {
//            gettimeofday(&startTime,NULL);
//            programSet->startTime[timerNum] = startTime;
//            programSet->timerProcessType[timerNum] = TIMER_PROCESS_START;
//        }
//    }
//    else if(TIMER_PROCESS_STOP == processType)
//    {
//        if((TIMER_PROCESS_STOP == programSet->timerProcessType[timerNum]))
//        {

//        }
//        else
//        {
//            getCurrentTime(timerNum);
//            programSet->timerProcessType[timerNum] = TIMER_PROCESS_STOP;
//        }
//    }
//    else if(TIMER_PROCESS_RESET == processType)//在ｓｔａｒｔ和Ｒｅｇ模式下都能ｒｅｓｅｔ
//    {
////                programSet->timerOverFlowType[timerNum] = TIMER_OVER_FLOW_NONE;

//        gettimeofday(&startTime,NULL);

//        programSet->countTimer[timerNum] = 0.0;
//        programSet->startTime[timerNum] = startTime;
//        programSet->endTime[timerNum] = startTime;
////        programSet->timerProcessType[timerNum] = TIMER_PROCESS_RESET;
//    }

//   updateTimerReg();

//   return 0;
//}

//void ProgramEngine::runChildProgram()
//{
//    qDebug() << "ProgramEngine::runChildProgram,emit signal_runProgram ";
//    emit signal_runProgram();
//}

void ProgramEngine::initialParameter(int moveTypeIn, EN_CONTROL_TYPE runModelIn)
{
    moveType = moveTypeIn;
    runModel = runModelIn;
/*    oneStepEnableFlag = oneStepOrContinuousFlagIn*/;
}

void ProgramEngine::childTaskOpenFile(QString runFileName, int childTaskIndex)
{
    emit childTaskOpenFile_signal(runFileName,childTaskIndex);
}

void ProgramEngine::childTaskRun(int childTaskIndex)
{
    emit childTaskRun_signal(childTaskIndex);
}

void ProgramEngine::childTaskOpenFile_slot(QString fileName,int childTaskIndex)
{
    if(taskIndex!=childTaskIndex)
    {
        qDebug()<<"childTaskOpenFile_slot,this is not my task,my taskIndex="<<taskIndex<<"childTaskIndex="
               <<childTaskIndex;
        return ;
    }

    openFile(fileName);
    setCurrentProgramName(fileName);


}

int ProgramEngine::openFile(QString strFileName)
{

    qDebug()<<" ProgramEngine::openFile,strFileName=" <<strFileName;

    #ifdef TEST_TIME_DEBUG
    gettimeofday(&openFileStartTime,NULL);
    #endif

    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    tmpFilePath = tmpFilePath+ROBOT_COMMON_NAME + QString::number(robotId)
            + ROBOT_PROGRAM_FLODER + strFileName;
//    currentFilePath=tmpFilePath;

    if(isMainThread)
    {
        if( engineStatus == ENUM_COMMAND_RUN )
        {
            //消息预留
            qDebug()<<"还有程序在运行！" ;
            return -1;
        }

//        if( programMcController->mcControllerInterpreter->getStatus()==ENUM_COMMAND_RUN)
//        {
//            //消息预留
//            qDebug()<<"算法:还在运行！"<<programMcController->mcControllerInterpreter->getStatus();
//            return -1;
//        }
    }
    if( tmpFilePath.isEmpty() )
    {
        //消息预留
        addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3008,robotId);
        qDebug()<<"程式名字为空! openFile( ? )";
        return -1;
    }

    if( !QFile::exists(tmpFilePath)  )
    {
        //消息预留
        addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3009,robotId,0,0,0,0,strFileName);
        qDebug()<<"程式文件不存在! openFile( ? )";
        return -1;
    }


    if(1==initialProgramSetFromProgramFile(tmpFilePath,strFileName))
    {
        qDebug()<<"sucess,openfile="<<tmpFilePath;
    }
    else
    {
        qDebug()<<"error,initialProgramSetFromProgramFile,openfile";
        return  -1;
    }
    return 1;
}

int ProgramEngine::openFile_clearPrivious(QString strFileName)
{
    //非停止状态，不允许外部打开程序。
    if(ENUM_COMMAND_RESET != engineStatus && ENUM_COMMAND_END != engineStatus)
    {
        addMsg( ENUM_MSG_WARNING,  "ProgramEngine", "ProgramEngine",  3035,  robotId );
        return -1;
    }
    fileMutex.lock();
    qDebug()<<"ProgramEngine::openFile_clearPrivious"<<strFileName;
    resetAllProgramSet();
    qDebug()<<"resetAllProgramSet();programSet->size="<<programSet->size();
    if(1!=openFile(strFileName))
    {
        qDebug()<<"error,openFile";
        qDebug()<<"programSet->size="<<programSet->size();
        fileMutex.unlock();
        return -1;
    }
    setCurrentProgramName(strFileName);
    fileMutex.unlock();


    return 1;
}

int ProgramEngine::openDefaultFile_clearPrivious()
{
    return openFile_clearPrivious(currentProgramName);
}

int ProgramEngine::initialProgramSetFromProgramFile(QString strFilePath,QString strFileNameIn)
{
    int endFlag = 0;
    qDebug() << "initialProgramSetFromProgramFile=*"<<strFilePath<<endl;

    #ifdef TEST_TIME_DEBUG
    gettimeofday(&readFileStartTime,NULL);
    #endif
    endFlag = readOneProgramFile(strFilePath);// 解释程式

    #ifdef TEST_TIME_DEBUG
    gettimeofday(&readFileEndTime,NULL);
    #endif

    if(1 == endFlag)
    {
        qDebug() << "error,readOneProgramFile";
        return 0;
    }

    #ifdef TEST_TIME_DEBUG
    gettimeofday(&processStartTime,NULL);
    #endif

    cmdStrListMutex.lock();
    endFlag = programSet->transferTextInstructionToStructData(cmdStrList,0,strFileNameIn,engineStatus,robotId);
    cmdStrListMutex.unlock();

    if( endFlag < 0 )
    {
        addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3010,  robotId ,   endFlag );
        return false;
    }
    else if( endFlag > 0 )
    {

        addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3010,  robotId ,   endFlag );
        return false;
    }

    #ifdef TEST_TIME_DEBUG
    gettimeofday(&processEndTime,NULL);
    #endif

    if( endFlag < 0 )
    {
        programSet->clear();
        qDebug() << "error,endFlag < 0 ";
        return 0;
    }


    updateProgramSetRegister();
    (*programSet)[0].thisProgramName = strFileNameIn;
    (*programSet)[0].runState = true;
    updateProgramSetRunInformation(0);

    #ifdef TEST_TIME_DEBUG
    gettimeofday(&openFileEndTime,NULL);
    qDebug()<<"open总时间"<<countIntervalTime(openFileStartTime,openFileEndTime);
    qDebug()<<"readOneProgramFile时间"<<countIntervalTime(readFileStartTime,readFileEndTime);
    qDebug()<<"transferTextInstructionToStructData时间"<<countIntervalTime(processStartTime,processEndTime);
    #endif

    if( endFlag > 0 )
    {
        addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3010,  robotId ,   endFlag );
        qDebug() << "error,endFlag > 0 ";
        return 0;
    }
//----------------------------------------
    //for webserver
//    if(1 == endFlag)
//    {
//        webobj.openfile_return( robotId, 0 );
//        return 0;
//    }

//    webobj.openfile_return( robotId, 1 );
    return 1;
}



int ProgramEngine::updateProgramSetRunInformation(int programIndex)
{
    int programSize = programSet->size();
    //vrep 6 axis core dump while call updateProgram()
    if(programSize<1|| programIndex>=programSize)
    {
        qDebug()<<"\n\n\n\n\ updateProgram, no program opened!"<<programSize<<programIndex;
        //realTimeSleep(1000);
        return 1;
    }

    lock();
    int updateSize = programUpdateDataList.programUpdateDataVector.size();
    if(updateSize != programSize)
    {
        programUpdateDataList.programUpdateDataVector.resize(programSize);
    }
    else
    {
        programUpdateDataList.programUpdateDataVector[programIndex].currentLineNum =
                (*programSet)[programIndex].runningLineInt;
        programUpdateDataList.programUpdateDataVector[programIndex].runState =
                (*programSet)[programIndex].runState;
        programUpdateDataList.programUpdateDataVector[programIndex].programName =
                (*programSet)[programIndex].thisProgramName;
    }
    unlock();

    return 0;
}

float ProgramEngine::getPcFlagValue()
{
    return pcFlagValue;
}

int ProgramEngine::setPcFlagValue(float valueIn)
{
    pcFlagValue=valueIn;
    return 1;
}

int ProgramEngine::runProgram()
{
    qDebug()<<"11111111 ProgramEngine::runProgram() in -----------------------";
    qDebug() << "==========QThread::currentThreadId()="<<QThread::currentThreadId()<<endl;

    if(isProgramEngineInitialOk==false)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3025, robotId );
         qDebug()<<"error,,,,,,,,,,,,isProgramEngineInitialOk==false !";
        return 0;
    }

    if( runModel==ENUM_CONTROL_FLEET )
    {
        programMcController->mcControllerInterpreter->allStartRun();
        addMsg( ENUM_MSG_REMIND, "ProgramEngine", "ProgramEngine",  3034, robotId );
        qDebug()<<"remind,,,,,,,,,,,,runModel==ENUM_CONTROL_FLEET !";
        return 0;
    }

    if( ( (runModel<ENUM_CONTROL_AUTO &&oneStepEnableFlag)
          || runModel==ENUM_CONTROL_AUTO
          || runModel==ENUM_CONTROL_EXT )
          && (engineStatus==ENUM_COMMAND_RUN  ) )
    {
            //，机器人运行状态，0 暂停    1 运行    2 停止    4 出错
            qDebug()<<"error,,,,,,,,,,,,机器人已经处于运行中!";

            return 1;
    }

    if(programSet->size()<1)
    {
        qDebug()<<"error,,,,,,,,,,,,,,,,,,no program,请选择一个程序文件运行！";
        return 1;
    }

    if( runModel<ENUM_CONTROL_AUTO )
    {
        oneStepEnableFlag=true;
    }


    if(ENUM_COMMAND_RESET == engineStatus || ENUM_COMMAND_END == engineStatus)
    {//停止状态进来
        fileMutex.lock();//防止程序没有加载完成。
        if(true==isMainThread)
        {
            resetEngineAll();
//            //重新加载一次文件，保证文件被修改后，可以自动生效。可能耗时很久导致ｇsoap超时断线？
//            openFile_clearPrivious(currentProgramName); 把行号也复位了
        }
        if(!setRunNumFlag)
        {
            (*programSet)[0].runningLineInt = 0;
            (*programSet)[0].runState = true;
        }
        pretreatMotion();
        setEngineStatusRun();
        fileMutex.unlock();
        emit runNewTask_signal(taskIndex);//hualei 放在前面会导致后面的代码无法运行，被抢占了。
        qDebug()<<"停止状态下，开始运行－－－－－";
    }
    else if(  engineStatus == ENUM_COMMAND_CEASE )
    {
        setEngineStatusRun();
        qDebug()<<"暂停状态下，继续运行－－－－－";
        return 1;
    } 

    return 0;
}

#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

int ProgramEngine::programEngineTaskLoop(int taskIndexIn)
{
    if(taskIndex!=taskIndexIn)
    {
        qDebug()<<"programEngineTaskLoop,this is not my task,my taskIndex="<<taskIndex<<"taskIndexIn="
               <<taskIndexIn;
        return -1;
    }
    qDebug()<<"ProgramEngine pid="<<gettid()<<"taskIndex"<<taskIndexIn;
    //调度模式不运行。
    if(ENUM_CONTROL_FLEET==runModel )
    {
        return -2;
    }
    isInRunningLoop=true;

    //mcController start run
    qDebug()<<"22222222  programEngineTaskLoop start!----------------------------"
           <<"===QThread::currentThreadId()===="
           <<QThread::currentThreadId()
          <<"===currentThread->currentThreadId()===="
          <<currentThread->currentThreadId()
          << "taskIndex="<<taskIndex
           <<"currentProgramName"<<currentProgramName<<"\n\n";
    qDebug()<<"runningLineFloat="<<runningLineFloat<<" ,engineStatus="<< engineStatus
            <<" ,runningLineInt=" <<(*programSet)[0].runningLineInt<<", manual_run_flag="
           <<oneStepEnableFlag ;
    engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_BEGINE;

    loopTimeCount=0;

    //loop
    while( engineStatus == ENUM_COMMAND_RUN || ENUM_COMMAND_CEASE==engineStatus )
    {

        while( engineStatus == ENUM_COMMAND_RUN && false==isErrorOccured)
        {
            loopTimeCount++;
            engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_LOOP;
            for(int programIndex = 0; programIndex < programSet->size(); programIndex++)
            {
                if(true == (*programSet)[programIndex].runState)
                {
                    if(ENUM_COMMAND_RUN != engineStatus)
                    {
                        break;
                    }
                    E_PROGRAM_LOOP_RESULT tmpProcessResult=processOneProgram(programIndex);
                    if(E_PROGRAM_LOOP_RESULT_NORMAL==tmpProcessResult)
                    {

                    }
                    else if(E_PROGRAM_LOOP_RESULT_ERROR==tmpProcessResult)
                    {
                        isErrorOccured=true;//保证出错后，不再继续运行指令。
                        break;
                    }
                    else
                    {
                        break;
                    }
                }

            }
            engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_LOOP_PROGRAM_INDEX_OUT;


            if(true==isRunningBreak())
            {
                break;
            }

            QCoreApplication::processEvents();
    //        if(0 == loopTimeCount%D_PROGRAM_ENGINE_SLEEP_FREQUENCY)
    //        {
    //            realTimeSleep(D_PROGRAM_ENGINE_SLEEP_TIME);
    //        }

        }
        realTimeSleep(D_PROGRAM_ENGINE_SLEEP_TIME);

    }

    isRunningBreak();
    timerInstructionLogic->reset();
    forInstructionLogic->reset();
    if(true==isMainThread)
    {
        motionInstructionLogic->reset();
        moveFreeInstructionLogic->reset();
    }

    programSetReset();
    updateProgramSetRunInformation(0);



    qDebug()<<"222222222222programEngineTaskLoop out!----------------------------"
           <<"===QThread::currentThreadId()===="
           <<QThread::currentThreadId()
          <<"===currentThread->currentThreadId()===="
          <<currentThread->currentThreadId()
          << "taskIndex="<<taskIndex
           <<"currentProgramName"<<currentProgramName<<"\n\n";
    isInRunningLoop=false;

    return 0;
}

bool ProgramEngine::isRunningBreak()
{
    //算法出现异常停止时，程序也需要停止。
    if(true==isMainThread)
    {
        if(ENUM_TRAJECTORY_STOPPED==programMcController->mcControllerInterpreter->getStatus())
        {
//            setEngineStatusStop();
//            internalCease();
            if(ENUM_COMMAND_RUN==engineStatus)
            {
                programEngineCease_noMotionThread();
            }
            return true;
        }
    }

    if(ENUM_COMMAND_CEASE == engineStatus)
    {
        internalCease();
        return true;
    }
    else if(ENUM_COMMAND_END == engineStatus)
    {
        setEngineStatusStop();
        return true;
    }
    else if(ENUM_COMMAND_RESET == engineStatus)
    {
        setEngineStatusStop();
        return true;
//        if(isMainThread)
//        {
//           resetEngineAll();
//        }
//        return true;
    }
    return false;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::processOneProgram(int programIndex)
{
    E_PROGRAM_LOOP_RESULT returnType=E_PROGRAM_LOOP_RESULT_NORMAL;
    engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_LOOP_PROGRAM_INDEX;
    engineDebugInfo.programIndex_currentRunning=programIndex;
    updateProgramSetRunInformation(programIndex);
    runningLineFloat = (*programSet)[programIndex].runningLineInt + 0.02;
    if(runningLineFloat >= (*programSet)[programIndex].count())
    {

        if(0 == programIndex)
        {
            if(isMainThread)
            {
                    setEngineStatusStop();
                    qDebug()<<"用户主程式执行完毕！ ！";
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3026, robotId   );
                    return E_PROGRAM_LOOP_RESULT_PROGRAM_FINISHED;
            }
            else
            {
                    setEngineStatusStop();
                    qDebug()<<"用户协线程执行完毕！ ！";
                    return E_PROGRAM_LOOP_RESULT_PROGRAM_FINISHED;
            }
        }

    }
    else if(0!=(*programSet)[programIndex].errorFlag)
    {
        qDebug()<<"程序指令错误,0!=(*programSet)[programIndex].errorFlag！！！";
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3027, robotId   );
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    else
    {
        programInstructionType = (*programSet)[programIndex].at(runningLineFloat).type;
    }

    //手动单步时，一条指令运行完后暂停
    if(  runModel<ENUM_CONTROL_AUTO && !oneStepEnableFlag  )
    {
        if(isMainThread)
        {
            programMcController->mcControllerInterpreter->allCease();
            qDebug()<<"processOneProgram 单步暂停－－－－－ "
                   << runningLineFloat
                   << engineStatus
                   << programMcController->mcControllerInterpreter->getStatus()
                      //<< programMcController->mcControllerInterpreter->movingLineNumber
                   << programMcController->mcControllerInterpreter->runningLineNumber
                      //   << programMcController->mcControllerInterpreter->isLineFinished
                   << programMcController->mcControllerInterpreter->getRunMode()
                   << programMcController->mcControllerInterpreter->getMoveType()
                  <<(*programSet)[0].runningLineInt;
        }
        engineStatus = ENUM_COMMAND_CEASE;
        return E_PROGRAM_LOOP_RESULT_CEASE_STOP ;
    }
    //运行完一条指令
    if( (runModel<ENUM_CONTROL_AUTO && moveType==ENUM_MOVE_ORDER_STEP && oneStepEnableFlag )
            ||  (runModel<ENUM_CONTROL_AUTO && moveType==ENUM_MOVE_HAND_SERIES && oneStepEnableFlag )
            || runModel==ENUM_CONTROL_AUTO
            || runModel==ENUM_CONTROL_EXT )
    {
        if(true == (*programSet)[programIndex].runState)
        {
            if( debugEnable && (true == (*programSet)[programIndex].breakPointFlag) )
            {
                qDebug()<<"调试模式......."<<runningLineFloat;
                for(int j = 0; j < (*programSet)[programIndex].breakPointList.size(); j++)
                {
                    if((int)runningLineFloat == (*programSet)[programIndex].breakPointList[j])
                    {
                        qDebug()<< "运行停止! "<<runningLineFloat ;//
                    }
                    else
                    {
                        realTimeSleep(250);
                        engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_LOOP_PROGRAM_INSTRUCTION;
                        returnType=processCurrentRunningInstructionOneTime(programIndex,programInstructionType);
                        if(E_PROGRAM_LOOP_RESULT_NORMAL != returnType)
                        {
                            if(E_PROGRAM_LOOP_RESULT_ERROR==returnType)
                            {
                                qDebug()<<getCurrentProgramName()<<"行号LIne="
                                       <<(*programSet)[programIndex].runningLineInt+1<<"run error";
                                addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",3032,robotId
                                       ,(*programSet)[programIndex].runningLineInt+1
                                       ,0,0,0,getCurrentProgramName());
                            }
                            engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_LOOP_PROGRAM_INSTRUCTION_OUT;
                            return returnType;
                        }
                        engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_LOOP_PROGRAM_INSTRUCTION_OUT;
                    }
                }
            }
            else
            {
                engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_LOOP_PROGRAM_INSTRUCTION;
                returnType=processCurrentRunningInstructionOneTime(programIndex,programInstructionType);
                if(E_PROGRAM_LOOP_RESULT_NORMAL != returnType)
                {
                    if(E_PROGRAM_LOOP_RESULT_ERROR==returnType)
                    {
                        qDebug()<<getCurrentProgramName()<<"行号LIne="
                               <<(*programSet)[programIndex].runningLineInt+1<<"run error";
                        addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",3032,robotId
                               ,(*programSet)[programIndex].runningLineInt+1
                               ,0,0,0,getCurrentProgramName());
                    }
                    engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_LOOP_PROGRAM_INSTRUCTION_OUT;
                    return returnType;
                }
                engineDebugInfo.stage=E_PROGRAM_ENGINE_RUN_LOOP_PROGRAM_INSTRUCTION_OUT;
            }
        }
        else
        {


        }
    }

    return returnType;
}
int ProgramEngine::setRunModel_forChildTask(int runModelIn)
{
    emit setRunModel_signal(runModelIn);
    return 1;

}

int ProgramEngine::setMoveType_forChildTask(int moveTypeIn)
{
    emit setMoveType_signal(moveTypeIn);
    return 1;
}

int ProgramEngine::clearAllMsg()
{
    quickStopFlag=false;
    taskManager->clearAllMsg();
    if(isMainThread)
    {
        motionInstructionLogic->clearAllMsg();
    }
    #ifdef D_USE_ROS
    if(isMainThread)
    {
        liftMechanismInstructionLogic->clearAllMsg();
        rollerInstructionLogic->clearAllMsg();
    }
    #endif
    return 1;
}

int ProgramEngine::clearAllMsg_self()
{
    quickStopFlag=false;
    isErrorOccured=false;
    return 1;
}

int ProgramEngine::setRunModel_slot(int runModelIn)
{
    qDebug()<<"ProgramEngine::setRunModel_slot,taskIndex="<<taskIndex;

    return setRunModel_taskManager((EN_CONTROL_TYPE)runModelIn,false);

}

int ProgramEngine::setMoveType_slot(int moveTypeIn)
{
    qDebug()<<"ProgramEngine::setMoveType_slot,taskIndex="<<taskIndex;

    return setMoveType_taskManager(moveTypeIn);
}


int ProgramEngine::readOneProgramFile(QString strFilePath)
{

    cmdStrListMutex.lock();
    cmdStrList.clear();

    if(isMainThread)
    {
        currentPointProList->clear();
        currentVehicleDPointList.clear();
    }
    cmdStrListMutex.unlock();


    ProgramTextTransfer programFransfer(robotId);
    QVector <PlRegStruct> regPlList_out;//测试放到文件里。　todo
    QVector <StackInfo> palletStackInfo_out;
    QVector <PointPro> pointList_out;
    QMap <int, DPoint> vehicleDPointListOut;
    cmdStrListMutex.lock();
    QString tmpVersion;
    int tmpKey=programFransfer.transferText(strFilePath,cmdStrList,pointList_out,regPlList_out,palletStackInfo_out,
                                 vehicleDPointListOut,tmpVersion);
    switch (tmpKey) {
    case -1:
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3008, robotId);
        break;
    case -2:
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3009, robotId);
        break;
    case -3:
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3009, robotId);
        break;
    case -4:
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3043, robotId);
        break;

    default:
        break;
    }
    programFileVersionInfo=tmpVersion.toInt();
    cmdStrListMutex.unlock();

    if(isMainThread)
    {
        qDebug()<< "openfile  重新取点位数据! ";
        currentVehicleDPointList=vehicleDPointListOut;
        *currentPointProList=pointList_out;
        regPlList=regPlList_out;
//        currentPalletStackInfo=palletStackInfo_out;
        //将解析出来的普通点的数据发送到ｐｏｉｎｔＤａｔａ
        pointData->savePoint2List(*currentPointProList);
        allPointProList->append(*currentPointProList);
        allVehicleDPointList.append(currentVehicleDPointList);
        //将解析出来的码垛点的数据发送到ｐｏｉｎｔＤａｔａ
//        pointData->savePalletPoint2List(currentPalletStackInfo);
//        allStackInfoList.append(currentPalletStackInfo);
    }

    return 0;
}


//void ProgramEngine::writeProgramFile(QString strFilePath)
//{
//    fData.writeProgramFile( strFilePath );
//}


PointPro ProgramEngine::parsePRData(int programNum)
{
    return palInstructionLogic->parsePRData(programNum);
}

float ProgramEngine::parseRegisterData(int programNum)
{
    return palInstructionLogic->parseRegisterData(programNum);
}

QVector <int> ProgramEngine::parsePLData(int programNum)
{
    return palInstructionLogic->parsePLData(programNum);
}


E_PROGRAM_LOOP_RESULT ProgramEngine::startMotion(int programNum, int startLine)
{
    return motionInstructionLogic->startMotion(programNum,startLine);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startAxisMove(int programNum, int startLine)
{
    return motionInstructionLogic->startAxisMove(programNum,startLine,(*programSet)[programNum].at(startLine).axisMove);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startFetchWeldPoint(int programNum, int startLine)
{
    return laserCameraInstructionLogic->startFetchWeldPoint(programNum,startLine,(*programSet)[programNum].at(startLine).laserCameraPointFetchInstruction);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startLaserCameraControl(int programNum, int startLine)
{
    return laserCameraInstructionLogic->startLaserCameraControl(programNum,startLine,(*programSet)[programNum].at(startLine).laserCameraCtrlInstruction);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startLaserCameraTrack(int programNum, int startLine)
{
    return laserCameraInstructionLogic->startLaserCameraTrack(programNum,startLine,(*programSet)[programNum].at(startLine).laserCameraTrackInstruction);
}


//E_PROGRAM_LOOP_RESULT ProgramEngine::startMotionSkip(int programNum, MotionInstruction motionInstruct)
//{
//    return motionInstructionLogic->startMotionSkip(programNum,motionInstruct);

//}

E_PROGRAM_LOOP_RESULT ProgramEngine::startPause(int programNum, PauseInstruction pauseInstruct)
{
    return pauseInstructionLogic->startPause(programNum,pauseInstruct);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startAbort(int programNum, AbortInstruction abortInstruct)
{
    return abortInstructionLogic->startAbort(programNum,abortInstruct);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startEnd(int programNum, EndInstruction endInstruct)
{
    return endInstructionLogic->startEnd(programNum,endInstruct);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startUalm(int programNum, UalmInstruction ualmInstruct)
{

    return ualmInstructionLogic->startUalm(programNum,ualmInstruct);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startMessage(int programNum, MessageInstruction messageInstruct)
{
    return messageInstructionLogic->startMessage(programNum,messageInstruct);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startOverride(int programNum, OverrideInstruction overrideInstruct)
{
    return overrideInstructionLogic->startOverride(programNum,overrideInstruct);

}

//int ProgramEngine::getCurrentTime(int timerNum)
//{
//    gettimeofday(&endTime,NULL);
//    programSet->endTime[timerNum] = endTime;

//    float timeCount = 0.0;

//    float tmp1 = programSet->endTime[timerNum].tv_sec - programSet->startTime[timerNum].tv_sec;
//    float tmp2 =(programSet->endTime[timerNum].tv_usec - programSet->startTime[timerNum].tv_usec)/MICROSECOND_TIME ;

//    timeCount = tmp1 + tmp2;

////   qDebug()  <<__FUNCTION__<<"=timerNum====" <<timerNum<<"***tmp1***"<<tmp1<<"***tmp2*****"<<tmp2<<"====timeCount====="<<timeCount<<"\n\n\n\n\n";

////    timeCount = programSet->endTime[timerNum].tv_sec + programSet->endTime[timerNum].tv_usec/MICROSECOND_TIME
////            - programSet->startTime[timerNum].tv_sec - programSet->startTime[timerNum].tv_usec/MICROSECOND_TIME;

//    programSet->countTimer[timerNum] += timeCount ;
//    programSet->startTime[timerNum] = endTime;

//    if(OVER_FLOW_MAX_TIME > programSet->countTimer[timerNum])
//    {
////        programSet->timerOverFlowType[timerNum] = TIMER_OVER_FLOW_NONE;
//    }
//    else
//    {
////        programSet->timerOverFlowType[timerNum] = TIMER_OVER_FLOW_YET;
//    }

////    return (*programSet)[programNum].timerOverFlowType[timerNum];
//    return 0;
//}

//int ProgramEngine::getCurrentUpdateTime(int timerNum)
//{
//    struct timeval updateEndTime;
//    gettimeofday(&updateEndTime,NULL);
//    programUpdateDataList.endTime[timerNum] = updateEndTime;

//    float timeCount = 0.0;

//    float tmp1 = programUpdateDataList.endTime[timerNum].tv_sec - programUpdateDataList.startTime[timerNum].tv_sec;
//    float tmp2 =(programUpdateDataList.endTime[timerNum].tv_usec - programUpdateDataList.startTime[timerNum].tv_usec)/MICROSECOND_TIME ;

//    timeCount = tmp1 + tmp2;

////   qDebug()  <<__FUNCTION__<<"=timerNum====" <<timerNum<<"***tmp1***"<<tmp1<<"***tmp2*****"<<tmp2<<"====timeCount====="<<timeCount<<"\n\n\n\n\n";

////    timeCount = programSet->endTime[timerNum].tv_sec + programSet->endTime[timerNum].tv_usec/MICROSECOND_TIME
////            - programSet->startTime[timerNum].tv_sec - programSet->startTime[timerNum].tv_usec/MICROSECOND_TIME;

//    programUpdateDataList.timerCount[timerNum] += timeCount ;
//    programUpdateDataList.startTime[timerNum] = updateEndTime;

//    if(OVER_FLOW_MAX_TIME > programUpdateDataList.timerCount[timerNum])
//    {
////        programSet->timerOverFlowType[timerNum] = TIMER_OVER_FLOW_NONE;
//    }
//    else
//    {
////        programSet->timerOverFlowType[timerNum] = TIMER_OVER_FLOW_YET;
//    }

////    return (*programSet)[programNum].timerOverFlowType[timerNum];
//    return 0;
//}

void ProgramEngine::processWaitTimeOut(int programNum, waitInstruction waitInstruct)
{
   return waitInstructionLogic->processWaitTimeOut(programNum,waitInstruct);
}

bool ProgramEngine::countWaitTime(float waitTime)
{
    return timerInstructionLogic->countWaitTime(waitTime);
}

void ProgramEngine::cmdError(int programNum)
{
//    (*programSet)[programNum].runningLineInt = COMMAND_ERROR;
//    setEngineStatusStop();导致行号为０
    ceaseProgram();
    (*programSet)[programNum].errorFlag=1;
}

int ProgramEngine::updateProgramSetRegister()
{
    if(NULL != regData)
    {
       bool returnFlag1,returnFlag2,returnFlag3,returnFlag4;
       programSet->regRList = regData->returnRValue(returnFlag1);
       programSet->regPrList = regData->returnPrValue(returnFlag2);
       programSet->regPlList = regData->returnPlValue(returnFlag3);
       programSet->countTimer = regData->returnTimerValue(returnFlag4);
       if(returnFlag1==true && returnFlag2==true &&returnFlag3==true &&returnFlag4==true)
       {
           isProgramEngineInitialOk=true;
       }
       return 1;
    }
    else
    {
        return 0;
    }

}

//int ProgramEngine::updateTimerReg()
//{
//    lock();
////    programUpdateDataList.startTime.resize(programSet->startTime.size());
////    programUpdateDataList.endTime.resize(programSet->endTime.size());
////    programUpdateDataList.timerProcessType.resize(programSet->timerProcessType.size());
////    programUpdateDataList.timerCount.resize(programSet->countTimer.size());
//    programUpdateDataList.startTime = programSet->startTime;
//    programUpdateDataList.endTime = programSet->endTime;
//    programUpdateDataList.timerProcessType = programSet->timerProcessType;
//    programUpdateDataList.timerCount = programSet->countTimer;
//    unlock();
//}


//在使用ｔｉｍｅｒ时间时需要调用gettimeofday(&endTime,NULL);来获取当前时间，从而获取时间间隔差
E_PROGRAM_LOOP_RESULT ProgramEngine::startTimerCmd(int programNum, TimerInstruction timerInstruct)
{
    return timerInstructionLogic->startTimerCmd(programNum,timerInstruct);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startRsr(int programNum, RsrInstruction rsrInstruct)
{
    return rsrInstructionLogic->startRsr(programNum,rsrInstruct);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startAnnotation(int programNum)
{
    calculateNextLineToRun(programNum);

    E_PROGRAM_LOOP_RESULT endFlag = isTheLastInstruction(programNum);

    return endFlag;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startGripper(int programNum, GripperInstruction gripperInstruct)
{
   return gripperInstructionLogic->startGripper(programNum,gripperInstruct);
}


E_PROGRAM_LOOP_RESULT ProgramEngine::startWait(int programNum,waitInstruction waitInstruct)
{
    return waitInstructionLogic->startWait(programNum,waitInstruct);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startMoveMaganetic(int programNum, int startLine)
{
    return moveMagneticInstructionLogic->startMoveMagnetic(programNum,(*programSet)[programNum].
                                                           at( startLine).moveMagInstruction);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startMoveFree(int programNum, int startLine)
{
    return moveFreeInstructionLogic->startMoveFree(programNum,(*programSet)[programNum].at( startLine).moveFreeInstruction,startLine);
}



E_PROGRAM_LOOP_RESULT ProgramEngine::startJump(int programNum,QString jumpLabelIn)
{
    return jumpInstructionLogic->startJump(programNum,jumpLabelIn);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startReg(int programNum, RegisterInstruction registerInstruct)
{
    return regInstructionLogic->startReg(programNum,registerInstruct);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startPrReg(int programNum, const PrRegisterInstruction &prRegisterInstruction)
{
    return regInstructionLogic->startPrReg(programNum,prRegisterInstruction);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startPlReg(int programNum, const PlRegisterInstruction &plRegisterInstruction)
{
     return regInstructionLogic->startPlReg(programNum,plRegisterInstruction);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startIo(int programNum,IoInstruction ioInstruct)
{
    return ioInstructionLogic->startIo(programNum,ioInstruct);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startLabel(int programNum)
{
    return labelInstructionLogic->startLabel(programNum);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startRun(int programNum,QString runProgramName)
{
//    qDebug() << "==========currentThreadId=============="<<QThread::currentThreadId()<<endl;

    E_PROGRAM_LOOP_RESULT returnType = E_PROGRAM_LOOP_RESULT_NORMAL;

    if(false==isMainThread)
    {
        cmdError(programNum);
        qDebug()<<"error--------------------ProgramEngine::startRun,false==isMainThread";
        addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",3030,robotId,0,0,0,0,runProgramName);
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    qDebug()<<"==run:"<<programNum<<runProgramName;
    int taskIndexOut;
    if(-1==taskManager->runTask(moveType,runModel,runProgramName,taskIndexOut))
    {
        cmdError(programNum);
        qDebug()<<"error--------------------ProgramEngine::startRun,D_MAX_TASK_LIMIT > childMount";
        addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",3022,robotId);
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    calculateNextLineToRun(programNum);
    returnType = isTheLastInstruction(programNum);
    return returnType;
}

//void ProgramEngine::addTask(TaskManager &runTaskManager)
//{
//    lock();
//    taskManager = &runTaskManager;
//    unlock();
//}

void ProgramEngine::addRegRsrData(RegData &mainRegData,RsrInfo &tmpRsrInfo)
{
    regData = &mainRegData;
    rsrInfo = &tmpRsrInfo;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startCall(int programNum,QString callProgramName)
{
//    taskManager->modifyProgramEningeFileName(taskIndex,callProgramName);
    E_PROGRAM_LOOP_RESULT tmpKey=callInstructionLogic->startCall(programNum,callProgramName);
    if(isMainThread)
    {
        if(E_PROGRAM_LOOP_RESULT_NORMAL==tmpKey)
        {
            setRunningNumber_virtualLineControl(0);
            setRunningNumber_robotArmControl(0);
        }
    }
    return tmpKey;

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startFor(int programNum,ForInstruction forInstruct)
{
    return forInstructionLogic->startFor(programNum,forInstruct);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startEndfor(int programNum)
{
    return forInstructionLogic->startEndfor(programNum);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startPal(int programNum,PalInstruction palInstruct)
{
    return palInstructionLogic->startPallet(programNum,palInstruct);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startSelect(int programNum, SelectInstruction selectInstruction)
{
    return selectInstructionLogic->startSelect(programNum,selectInstruction);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::caseAnd(int programNum,IfInstruction andIfInstruction)
{
    return ifInstructionLogic->caseAnd(programNum,andIfInstruction,runningLineFloat);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::caseOr(int programNum,IfInstruction orIfInstruction)
{
    return ifInstructionLogic->caseOr(programNum,orIfInstruction,runningLineFloat);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::caseNeither(int programNum,IfInstruction nIfInstruction)
{
    return ifInstructionLogic->caseNeither(programNum,nIfInstruction,runningLineFloat);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startCoorDinate(int programNum, CoorDinateInstruction coorInstruct)
{
    return coordinateInstructionLogic->startCoorDinate(programNum,coorInstruct);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startErr(int programNum)
{
    E_PROGRAM_LOOP_RESULT returnType = E_PROGRAM_LOOP_RESULT_NORMAL;
    calculateNextLineToRun(programNum);
    returnType = isTheLastInstruction(programNum);
    return returnType;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::isTheLastInstruction(int programNum)
{
    E_PROGRAM_LOOP_RESULT returnType = E_PROGRAM_LOOP_RESULT_NORMAL;

    if((*programSet)[programNum].runningLineInt >= (*programSet)[programNum].count()
            || D_THE_LAST_LINE==(*programSet)[programNum].runningLineInt)
    {
        // qDebug() << "===size====="<<programSet->size()<<"\n\n\n\n\n";
        bool isAllFinished=true;
        int fatherProgramIndex=0;
        for(int i = 0; i < programSet->size();i++)
        {
            if((*programSet)[programNum].fatherProgramName == programSet->at(i).thisProgramName)
            {
                setCurrentProgramName(programSet->at(i).thisProgramName);
                fatherProgramIndex=programNum;
                isAllFinished=false;
                (*programSet)[i].runState = true;
                (*programSet)[programNum].runState = false;
                (*programSet)[i].runningLineInt++;
                if(isMainThread)
                {
                    setRunningNumber_virtualLineControl((*programSet)[i].runningLineInt);
                    setRunningNumber_robotArmControl((*programSet)[i].runningLineInt);
                }

                if(0 < allPointProList->size())
                {
                    *currentPointProList = (*allPointProList)[i];
                }

                if(0 < allVehicleDPointList.size())
                {
                    currentVehicleDPointList = allVehicleDPointList[i];
                }


                if(NULL != pointData)
                {
                    pointData->savePoint2List(*currentPointProList);
//                    pointData->savePalletPoint2List(currentPalletStackInfo);
                }
                 // qDebug()<<"==111111===programCountNum===="<<programCountNum<<"\n\n\n\n\n\n\n\n\n";
            }
            else
            {
                (*programSet)[i].runState = false;
            }

        }
        if(true==isAllFinished)
        {
            setEngineStatusStop();
            if(D_THE_LAST_LINE==(*programSet)[programNum].runningLineInt)
            {
                qDebug() << "主程序运行到最后一行，运行停止===programNum="<<programNum;
                addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3031,robotId
                       ,0,0,0,0,(*programSet)[programNum].thisProgramName);
            }
            else
            {
                qDebug() << "主程序运行完ＥＮＤ===programNum="<<programNum;
                addMsg(ENUM_MSG_WARNING,"ProgramEngine","ProgramEngine",3029,robotId
                       ,0,0,0,0,(*programSet)[programNum].thisProgramName);
            }
        }
        else
        {
            qDebug() << "ＣＡＬＬ子程序运行到最后一行，返回父程序programNum="<<programNum;
        }

        pcFlagValue = 0.0;

        if(0 == programNum)
        {           
            (*programSet)[programNum].runState = true;
        }

        updateProgramSetRunInformation(programNum);

    }

    return returnType;
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startMusic(int programNumIn, int startLine)
{

    return musicInstructionLogic->startMusic(programNumIn, (*programSet)[programNumIn].at(startLine).musicInstruction);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startCharging(int programNumIn, int startLine)
{
    return chargingInstructionLogic->startCharging(programNumIn, (*programSet)[programNumIn].at(startLine).chargingInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startLiftMechanism(int programNumIn, int startLine)
{
    return liftMechanismInstructionLogic->startLiftMechanism(programNumIn, (*programSet)[programNumIn].at(startLine).liftMechanismInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startTcpStart(int programNumIn, int startLine)
{
    return tcpStartInstructionLogic->startTcpStart(programNumIn, (*programSet)[programNumIn].at(startLine).tcpStartInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startTcpRead(int programNumIn, int startLine)
{
    return tcpReadInstructionLogic->startTcpRead(programNumIn, (*programSet)[programNumIn].at(startLine).tcpReadInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startTcpWrite(int programNumIn, int startLine)
{
    return tcpWriteInstructionLogic->startTcpWrite(programNumIn, (*programSet)[programNumIn].at(startLine).tcpWriteInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startLoadMap(int programNumIn, int startLine)
{
    return loadMapInstructionLogic->startLoadMap(programNumIn, (*programSet)[programNumIn].at(startLine).loadMapInstruction, startLine);
}
E_PROGRAM_LOOP_RESULT ProgramEngine::startCheckBattery(int programNumIn, int startLine)
{
    return checkBatteryInstructionLogic->startCheckBatteryLevel(programNumIn, (*programSet)[programNumIn].at(startLine).checkBatteryInstruction, startLine);
}
E_PROGRAM_LOOP_RESULT ProgramEngine::startWaitCommand(int programNumIn, int startLine)
{
    return waitCommandInstructionLogic->startWaitCommand(programNumIn, (*programSet)[programNumIn].at(startLine).waitCommandInstruction, startLine);
}
E_PROGRAM_LOOP_RESULT ProgramEngine::startSetOdom(int programNumIn, int startLine)
{
    return setOdomInstructionLogic->startSetOdom(programNumIn, (*programSet)[programNumIn].at(startLine).odometryResetInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startRobotMove(int programNumIn, int startLine)
{
    return robotMoveInstructionLogic->startRobotMove(programNumIn, (*programSet)[programNumIn].at(startLine).robotMoveInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startRoller(int programNumIn, int startLine)
{
    return rollerInstructionLogic->startRoller(programNumIn, (*programSet)[programNumIn].at(startLine).rollerInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startModbusStart(int programNumIn, int startLine)
{
    return modbusStartInstructionLogic->startModbusStart(programNumIn, (*programSet)[programNumIn].at(startLine).modbusStartInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startModbusRead(int programNumIn, int startLine)
{
    return modbusReadInstructionLogic->startModbusRead(programNumIn, (*programSet)[programNumIn].at(startLine).modbusReadInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startModbusWrite(int programNumIn, int startLine)
{
    return modbusWriteInstructionLogic->startModbusWrite(programNumIn, (*programSet)[programNumIn].at(startLine).modbusWriteInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startVisionRun(int programNumIn, int startLine)
{
    return visionRunLogic->startVisionRun(programNumIn, (*programSet)[programNumIn].at(startLine).visionRunInstruction, startLine);

}

E_PROGRAM_LOOP_RESULT ProgramEngine::startVisionClearData(int programNumIn, int startLine)
{
    return visionRunLogic->startVisionClearData(programNumIn, (*programSet)[programNumIn].at(startLine).visionClearDataInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startVisionGetData(int programNumIn, int startLine)
{
    return visionRunLogic->startVisionGetData(programNumIn, (*programSet)[programNumIn].at(startLine).visionGetDataInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startVisionTrigger(int programNumIn, int startLine)
{
    return visionRunLogic->startVisionTrigger(programNumIn, (*programSet)[programNumIn].at(startLine).visionTriggerInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startTrackStart(int programNumIn, int startLine)
{
    return trackStartLogic->startTrackStart(programNumIn, (*programSet)[programNumIn].at(startLine).trackStartInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startTrackEnd(int programNumIn, int startLine)
{
   return trackStartLogic->startTrackEnd(programNumIn, (*programSet)[programNumIn].at(startLine).trackEndInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startTrackGetData(int programNumIn, int startLine)
{
    return trackStartLogic->startTrackGetData(programNumIn, (*programSet)[programNumIn].at(startLine).trackGetDataInstruction, startLine);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startWeldStart(int programNumIn, int startLine)
{
    return weldStartLogic->startWeldStart(programNumIn, (*programSet)[programNumIn].at(startLine).weldStartInstruction);
}

E_PROGRAM_LOOP_RESULT ProgramEngine::startWeldEnd(int programNumIn, int startLine)
{
    return weldStartLogic->startWeldEnd(programNumIn, (*programSet)[programNumIn].at(startLine).weldEndInstruction);
}
E_PROGRAM_LOOP_RESULT ProgramEngine::startWeave(int programNumIn, int startLine)
{
    return weaveLogic->startWeave(programNumIn, (*programSet)[programNumIn].at(startLine).weaveInstruction);
}
E_PROGRAM_LOOP_RESULT ProgramEngine::startWeaveEnd(int programNumIn, int startLine)
{
    return weaveLogic->startWeaveEnd(programNumIn, (*programSet)[programNumIn].at(startLine).weaveEndInstruction);
}
E_PROGRAM_LOOP_RESULT ProgramEngine::startTrackWeldTast(int programNumIn, int startLine)
{
    return trackWeldTastLogic->startTrackWeldTast(programNumIn, (*programSet)[programNumIn].at(startLine).trackWeldTastInstruction);
}
E_PROGRAM_LOOP_RESULT ProgramEngine::startTrackWeldEnd(int programNumIn, int startLine)
{
    return trackWeldEndLogic->startTrackWeldEnd(programNumIn, (*programSet)[programNumIn].at(startLine).trackWeldEndInstruction);
}
E_PROGRAM_LOOP_RESULT ProgramEngine::startWatchDI(int programNumIn, int startLine)
{
    return watchDILogic->startWatchDI(programNumIn, (*programSet)[programNumIn].at(startLine).watchDIInstruction);
}

int ProgramEngine::ceaseProgram()
{

    //如果处于焊接中，则把暂停当做停止处理．
    if(true==isMainThread)
    {
        qDebug()<<"ProgramEngine::ceaseProgram";
        weldStartLogic->stopWeld();
        //不能连续停２次，否则会导致立刻减速为０
        programMcController->mcControllerInterpreter->allCease();
    }

    if(ENUM_COMMAND_RESET!=engineStatus)
    {
        engineStatus = ENUM_COMMAND_CEASE;

        oneStepEnableFlag=false;

        taskManager->setAllTaskCease();
    }


    return 1;
}

int ProgramEngine::quickStop()
{
    quickStopFlag=true;
//    qDebug()<<"ProgramEngine::ceaseProgram() in,,,"<<__LINE__;
    if(isMainThread)
    {
        programMcController->mcControllerInterpreter->quickStop();//算法sstop
        weldStartLogic->stopWeld();
    }
    if(ENUM_COMMAND_RESET!=engineStatus)
    {
        engineStatus = ENUM_COMMAND_CEASE;
        oneStepEnableFlag=false;
        taskManager->setAllTaskCease();

    }

//    qDebug()<<"ProgramEngine::quickStop() out,,,"<<__LINE__;
    return 1;
}

int ProgramEngine::setEngineStatus_fleet(STOP_COMMAND_TYPE statusIn)
{
    engineStatus = statusIn;
    return 1;
}

int ProgramEngine::setEngineStatusRun()
{
    if(isMainThread)
    {
        programMcController->mcControllerInterpreter->allStartRun();
    }

    engineStatus = ENUM_COMMAND_RUN;
    taskManager->setAllTaskRun();

    return 1;
}

int ProgramEngine::setEngineStatusStop()
{
    engineStatus = ENUM_COMMAND_RESET;
    taskManager->setAllTaskStop();

    return 1;
}

int ProgramEngine::resetEngineAll()
{    
//    qDebug()<<"==ProgramEngine::resetEngineAll()==="<<"===,QThread::currentThreadId()="
//           <<QThread::currentThreadId()<<"\n\n";

    if(false==isMainThread)
    {
        qDebug()<<"ProgramEngine::resetEngineAll() failed, not mainthread!";
        return -1;
    }

    setEngineStatusStop();
    engineStop();



    return 1;
}

int ProgramEngine::resetAllProgramSet()
{

    programSet->clear();
//    programCountSize=0;
    allPointProList->clear();
    allVehicleDPointList.clear();
//    allStackInfoList.clear();

    return 1;
}

int ProgramEngine::restoreDefaultPogramSet()
{
    programSet->resize(1);
    (*programSet)[0].runState=true;
//    programCountSize=0;
    allPointProList->resize(1);
    allVehicleDPointList.resize(1);
//    allStackInfoList.resize(1);
    currentProgramName=(*programSet)[0].thisProgramName;
    return 1;
}

int ProgramEngine::setTaskRunState()
{

    if( runModel<ENUM_CONTROL_AUTO )
    {
        oneStepEnableFlag=true;
    }
    engineStatus = ENUM_COMMAND_RUN;
    return 1;
}

int ProgramEngine::setTaskCeaseState()
{
    engineStatus = ENUM_COMMAND_CEASE;
    return 1;
}

int ProgramEngine::setTaskStopState()
{
    engineStatus = ENUM_COMMAND_RESET;
    setRunningNumber_virtualLineControl(0);
    setRunningNumber_robotArmControl(0);
    return 1;
}

//int ProgramEngine::resetTaskManager()
//{
//    lock();
//    taskManager->programEngineList.clear();
//    ProgramEngineInfo tmpProgramEngine;
//    tmpProgramEngine.fileName = currentProgramName;
//    tmpProgramEngine.ProgramEnginePointer = this;
//    tmpProgramEngine.runningThread = currentThread;
//    taskManager->programEngineList.append(tmpProgramEngine);
//    unlock();
//}

int ProgramEngine::releaseHoldOn()
{
    //等待算法結束?
    if(isMainThread)
    {
        programMcController->mcControllerInterpreter->quickStop();//算法sstop
    }

    pcFlagValue = 0.0;

    oneStepEnableFlag=false;

    runningLineFloat = 0.0;

    qDebug()<<"ProgramEngine::releaseHoldOn()"<<__LINE__<<programSet->size();
//    engineStatus = ENUM_COMMAND_RESET;
    if( programSet->size()>0 ) {
        (*programSet)[0].runningLineInt = 0;
 //       (*programSet)[0].pc+0.02;
//        (*programSet)[0].runState = false;
        for(int i =0; i < programSet->size();i++)
        {
            updateProgramSetRunInformation(i);
            for(int j = 0; j < (*programSet)[i].count();j++)
            {
                (*programSet)[i][j].forInstruct.forCompleteFlag = true;
            }
        }
    }

    qDebug()<<"ProgramEngine::releaseHoldOn()"<<__LINE__;

    return 1;
}

int ProgramEngine::setStartLine(int lineNum)
{
    qDebug()<<"ProgramEngine::setStartLine";
    if(ENUM_COMMAND_RESET!=engineStatus )
    {
        addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",3039,robotId);
        return -1;
    }

    setRunNumFlag = true;

    (*programSet)[0].runningLineInt = lineNum;
    runningLineFloat = (*programSet)[0].runningLineInt+0.02;

    updateProgramSetRunInformation(0);

    pcFlagValue = 0.0;
//    if(MOTION == (*programSet)[0][lineNum].type)
//    {
//    getSetMotionBlock(lineNum);
//    }
//    else
//    {
//        lock();
//        (*programSet)[0].pc = lineNum;
//        pCount = (*programSet)[0].pc+0.02;
//        unlock();
//    }

    return 1;


}

int ProgramEngine::weldWorkingMonitor()
{
    return weldStartLogic->weldWorkingMonitor();
}

int ProgramEngine::setMoveType(int moveTypeIn)
{
//    return taskManager->setMoveType(moveTypeIn);
    setMoveType_forChildTask(moveTypeIn);
    return setMoveType_taskManager(moveTypeIn);
}

int ProgramEngine::setMoveType_taskManager(int moveTypeIn)
{
    moveType=moveTypeIn;
    if(isMainThread)
    {
        programMcController->mcControllerInterpreter->switchMoveType(moveTypeIn);
    }
    return 1;
}

int ProgramEngine::loadPointAndPalInfo(int index)
{
    if(false==isMainThread)
    {
        return -1;
    }

    *currentPointProList=(*allPointProList)[index];
    currentVehicleDPointList=allVehicleDPointList[index];
//    currentPalletStackInfo=allStackInfoList[index];

    //将解析出来的普通点的数据发送到ｐｏｉｎｔＤａｔａ
    if(0 < currentPointProList->size())
    {
        pointData->savePoint2List(*currentPointProList);
        qDebug()<< "loadPointAndPalInfo ,pointData change点位数据! ";
    }

    return 1;

}


int ProgramEngine::getSetMotionBlock(int startLineNum)
{
    return motionInstructionLogic->getSetMotionBlock(startLineNum);

}

int ProgramEngine::addBreakPoint(QString fileName, int lineNum)
{
//    //   int runFlag = 0;
//    for(int i = 0;i < programSet->size();i++)
//    {
//        if(fileName == (*programSet)[i].programName)
//        {
//            (*programSet)[i].breakPointFlag = true;
//            (*programSet)[i].breakPointList.append(lineNum);
//            break;
//        }
//        else
//        {
//            if(i == programSet->size() - 1)
//            {
//                //               runFlag = 1;//标志位，当遍历完当前线程的所有程序而没有找到匹配的程序名称，则将标志位置１，表明当前线程没有该程序，需要到子线程中查找
//            }
//        }
//    }

//    if(taskManager->programEngineList.size())
//    {
//        for(int i = 0; i < taskManager->programEngineList.size();i++)
//        {
//            for(int j = 0; j < taskManager->programEngineList[i].ProgramEnginePointer->programSet->size();j++)
//            {
//                if(fileName == taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].programName)
//                {
//                    taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointFlag = true;
//                    taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointList.append(lineNum);
//                    break;
//                }
//            }
//        }
//    }

}

int ProgramEngine::deleteBreakPoint(QString fileName,int lineNum)
{

//    for(int i = 0; i < programSet->size();i++)
//    {
//        if(fileName == (*programSet)[i].programName)
//        {
//            for(int j = 0;j < (*programSet)[i].breakPointList.size();j++)
//            {
//                if((*programSet)[i].breakPointList[j] == lineNum)
//                {
//                    (*programSet)[i].breakPointList.remove(j);
//                    if((*programSet)[i].breakPointList.size())
//                    {
//                        (*programSet)[i].breakPointFlag = true;
//                    }
//                    else
//                    {
//                        (*programSet)[i].breakPointFlag = false;
//                    }
//                    break;
//                }
//            }
//            break;
//        }
//    }



//    if(taskManager->programEngineList.size())
//    {
//        for(int i = 0; i < taskManager->programEngineList.size();i++)
//        {
//            for(int j = 0; j < taskManager->programEngineList[i].ProgramEnginePointer->programSet->size();j++)
//            {
//                if(fileName == taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].programName)
//                {
//                    for(int k = 0; k < taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointList.size();k++)
//                    {
//                        if(lineNum == taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointList[k])
//                        {
//                            taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointList.remove(k);
//                            if(taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointList.size())
//                            {
//                                taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointFlag = true;
//                            }
//                            else
//                            {
//                                taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointFlag = false;
//                            }
//                            break;
//                        }
//                    }
//                    break;
//                }
//            }
//        }
//    }

}

int ProgramEngine::deleteAllBreakPoint()
{
//    for(int i = 0; i < programSet->size();i++)
//    {
//        (*programSet)[i].breakPointFlag = false;
//        (*programSet)[i].breakPointList.clear();
//    }


//    if(taskManager->programEngineList.size())
//    {
//        for(int i = 0; i < taskManager->programEngineList.size();i++)
//        {
//            for(int j = 0; j < taskManager->programEngineList[i].ProgramEnginePointer->programSet->size();j++)
//            {
//                taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointFlag = false;
//                taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointList.clear();
//            }
//        }
//    }

}

int ProgramEngine::deleteSigleFileBreakPoint(QString fileName)
{
//    for(int i = 0; i < programSet->size();i++)
//    {
//        if((*programSet)[i].programName == fileName)
//        {
//            (*programSet)[i].breakPointFlag = false;
//            (*programSet)[i].breakPointList.clear();
//        }
//    }



//    if(taskManager->programEngineList.size())
//    {
//        for(int i = 0; i < taskManager->programEngineList.size();i++)
//        {
//            for(int j = 0; j < taskManager->programEngineList[i].ProgramEnginePointer->programSet->size();j++)
//            {
//                if(taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].programName == fileName)
//                {
//                    taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointFlag = false;
//                    taskManager->programEngineList[i].ProgramEnginePointer->(*programSet)[j].breakPointList.clear();
//                }
//            }
//        }
//    }

}

int ProgramEngine::clearMessage(Message engineMsg)
{

}

int ProgramEngine::pretreatMotion()
{
    return motionInstructionLogic->pretreatMotion();
}


int ProgramEngine::calculateNextLineToRun(int programNum)
{
    qDebug()<<"calculateNextLineToRun...";
    if(programNum >= programSet->size())
    {
        return -1;
    }

    if( runModel<ENUM_CONTROL_AUTO && moveType==ENUM_MOVE_ORDER_STEP)
    {       
        ++(*programSet)[programNum].runningLineInt;
        oneStepEnableFlag=false;
        if(isMainThread)
        {
            qDebug()<<"calculateNextLineToRun======15===========暂停"
                   << runningLineFloat
                   << engineStatus
                   << programMcController->mcControllerInterpreter->getStatus()
                      //<< programMcController->mcControllerInterpreter->movingLineNumber
                      //<< programMcController->mcControllerInterpreter->isLineFinished
                   << programMcController->mcControllerInterpreter->getRunMode()
                   << programMcController->mcControllerInterpreter->getMoveType();
        }
    }
    else if (runModel<ENUM_CONTROL_AUTO && moveType==ENUM_MOVE_HAND_SERIES)
    {
        ++(*programSet)[programNum].runningLineInt;
    }
    else if(  runModel==ENUM_CONTROL_AUTO || runModel==ENUM_CONTROL_EXT )
    {
        ++(*programSet)[programNum].runningLineInt;
    }

//    pCount = (*programSet)[programNum].pc+0.02;//更新行号(防止丢失精度)

//    qDebug() <<"***pCount***"<<pCount<<"----------(*programSet)[programNum].pc---------"<<(*programSet)[programNum].pc<<"\n\n";
}

E_PROGRAM_LOOP_RESULT ProgramEngine::processCurrentRunningInstructionOneTime(int programIndex,
                                                         INSTRUCTION_TYPE programInstructionTypeIn)
{
    E_PROGRAM_LOOP_RESULT returnType = E_PROGRAM_LOOP_RESULT_NORMAL;

    if(ENUM_COMMAND_RUN != engineStatus)
    {
        returnType = E_PROGRAM_LOOP_RESULT_CEASE_STOP;
    }
    else
    {
        int endFlag=programSet->examParse(programIndex,runningLineFloat);
        if( endFlag< 0)
        {
            addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3010,  robotId ,   endFlag );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

        if(programIndex >= programSet->size())
        {
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

        switch(programInstructionTypeIn)
        {
        case MOTION:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                if(programIndex >= programSet->size())
                {
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }
                else
                {

                    returnType = startMotion(programIndex,runningLineFloat);

                }
            }
        }
            break;
        case INS_AXIS_MOVE:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                if(programIndex >= programSet->size())
                {
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }
                else
                {

                    returnType = startAxisMove(programIndex,runningLineFloat);

                }
            }
        }
            break;
        case INS_LASER_CAMERA_FETCHPOINT:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                if(programIndex >= programSet->size())
                {
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }
                else
                {

                    returnType = startFetchWeldPoint(programIndex,runningLineFloat);

                }
            }
        }
            break;
        case INS_LASER_CAMERA_CTRL:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {

                    returnType = startLaserCameraControl(programIndex,runningLineFloat);
            }
        }
            break;
        case INS_LASER_CAMERA_TRACK:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {

                    returnType = startLaserCameraTrack(programIndex,runningLineFloat);
            }
        }
            break;
        case INS_LASER_SCAN:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {

                    returnType = laserCameraInstructionLogic->laserScanStart(programIndex,
                                    (*programSet)[programIndex].at(runningLineFloat).laserScanIns.isStart);
            }
        }
            break;
        case INS_BUFFER_MOVE:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {

                    returnType = returnType = motionInstructionLogic->bufferMove(programIndex,runningLineFloat,
                                                          (*programSet)[programIndex].at(runningLineFloat).bufferMoveIns.velocity);
            }
        }
            break;

        case WAIT:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startWait(programIndex,(*programSet)[programIndex].at(runningLineFloat).waitInstruct);
            }
        }
            break;
        case JUMP:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                switch((*programSet)[programIndex].at(runningLineFloat).jump.type)
                {
                case DIRECT:
                {
                    if(programIndex >= programSet->size())
                    {
                        return E_PROGRAM_LOOP_RESULT_ERROR;
                    }
                    else
                    {
                        QString tmpjumpLabel=(*programSet)[programIndex].at(runningLineFloat).jump.jumpLabelStr;
                        returnType = startJump(programIndex,tmpjumpLabel);
                    }
                }
                    break;
                case INDIRECT:
                {
                    if(programIndex >= programSet->size())
                    {
                        return E_PROGRAM_LOOP_RESULT_ERROR;
                    }
                    else
                    {
                        int tmpIndex=(*programSet)[programIndex].at(runningLineFloat)
                                .jump.jumpLabelStr.toInt()-1;
                        if(tmpIndex<0 || tmpIndex>=programSet->regRList.size())
                        {
                            addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3040, robotId, tmpIndex+1);
                            return E_PROGRAM_LOOP_RESULT_ERROR;
                        }
                        QString tmpjumpLabel=QString::number(programSet->regRList.at(tmpIndex));
                        returnType = startJump(programIndex,tmpjumpLabel);
                    }
                }
                    break;
                }
            }
        }
            break;
        case REG:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startReg(programIndex,(*programSet)[programIndex].at(runningLineFloat).reg);
            }
        }
            break;
        case INS_REG_PR:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startPrReg(programIndex,(*programSet)[programIndex].at(runningLineFloat).prRegisterInstruction);
            }
        }
            break;
        case INS_REG_PL:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startPlReg(programIndex,(*programSet)[programIndex].at(runningLineFloat).plRegisterInstruction);
            }
        }
            break;
        case IO:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startIo(programIndex,(*programSet)[programIndex].at(runningLineFloat).io);
            }
        }
            break;
        case LABEL:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startLabel(programIndex);
            }
        }
            break;
        case RUN:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else if(0==(*programSet)[programIndex].size() )
            {
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3042, robotId );
                qDebug()<<"call error,0==(*programSet)[programIndex].size ";
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
//                qDebug()<< "run-size=" << (*programSet)[programIndex].size << runningLineFloat;
                returnType = startRun(programIndex,
                                      (*programSet)[programIndex].at(runningLineFloat).runInstruct.programName);
            }
        }
            break;
        case CALL:
        {
            //                        for(int j = 0; j < programSet->size();j++)
            //                        {
            //                            if((*programSet)[i].at(pCount).call.programName == (*programSet)[j].programName)
            //                            {
            //                                 endFlag = startCall(i,(*programSet)[i].at(pCount).call.programName);
            //                            }
            //                        }
            if(programIndex >= programSet->size() )
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else if(0==(*programSet)[programIndex].size() )
            {
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3041, robotId );
                qDebug()<<"call error,0==(*programSet)[programIndex].size ";
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                startCall(programIndex,(*programSet)[programIndex].at(runningLineFloat).call.programName);
            }
        }
            break;
        case IF:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                updateProgramSetRegister();
                switch((*programSet)[programIndex].at(runningLineFloat).ifIns.andOrType)
                {
                case IF_AND:
                {
                    if(programIndex >= programSet->size())
                    {
                        return E_PROGRAM_LOOP_RESULT_ERROR;
                    }
                    else
                    {
                        returnType = caseAnd(programIndex,(*programSet)[programIndex].at(runningLineFloat).ifIns);
                    }
                }
                    break;
                case IF_OR:
                {
                    if(programIndex >= programSet->size())
                    {
                        return E_PROGRAM_LOOP_RESULT_ERROR;
                    }
                    else
                    {
                        returnType = caseOr(programIndex,(*programSet)[programIndex].at(runningLineFloat).ifIns);
                    }
                }
                    break;
                case NEITHER:
                {
                    if(programIndex >= programSet->size())
                    {
                        return E_PROGRAM_LOOP_RESULT_ERROR;
                    }
                    else
                    {
                        returnType = caseNeither(programIndex,(*programSet)[programIndex].at(runningLineFloat).ifIns);
                    }
                }
                    break;
                }
            }
        }
            break;
        case SELECT:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startSelect(programIndex,(*programSet)[programIndex].at(runningLineFloat).select);
            }
        }
            break;
        case FOR:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startFor(programIndex,(*programSet)[programIndex].at(runningLineFloat).forInstruct);
            }
        }
            break;
        case ENDFOR:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startEndfor(programIndex);
            }
        }
            break;
        case INS_PALLET:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startPal(programIndex,(*programSet)[programIndex].at(runningLineFloat).palInstruct);
            }
        }
            break;
        case COORDINATE:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startCoorDinate(programIndex,(*programSet)[programIndex].at(runningLineFloat).coorInstruct);
            }
        }
            break;
        case PAUSE:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startPause(programIndex,(*programSet)[programIndex].at(runningLineFloat).pauseInstruct);
            }
        }
            break;
        case ABORT:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startAbort(programIndex,(*programSet)[programIndex].at(runningLineFloat).abortInstruct);
            }
        }
            break;
        case END:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startEnd(programIndex,(*programSet)[programIndex].at(runningLineFloat).endInstruct);
            }
        }
            break;
        case UALM:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startUalm(programIndex,(*programSet)[programIndex].at(runningLineFloat).ualmInstruct);
            }
        }
            break;
        case MESSAGE:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startMessage(programIndex,(*programSet)[programIndex].at(runningLineFloat).messageInstruct);
            }
        }
            break;
        case OVERRIDE:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startOverride(programIndex,(*programSet)[programIndex].at(runningLineFloat).overrideInstruct);
            }
        }
            break;
        case TIMER:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startTimerCmd(programIndex,(*programSet)[programIndex].at(runningLineFloat).timerInstruct);
            }
        }
            break;
        case RSR:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startRsr(programIndex,(*programSet)[programIndex].at(runningLineFloat).rsrInstruct);
            }
        }
            break;
        case ANOTATION:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startAnnotation(programIndex);
            }
        }
            break;
        case GRIPPER:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startGripper(programIndex, (*programSet)[programIndex].at( runningLineFloat).gripperInstruct );
            }
        }
            break;
        case ERR://报错处理
        {
            //消息预留
            //指令错误不能识别
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startErr(programIndex);
            }
        }
            break;
        case MOVE_MAG://磁条导航
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startMoveMaganetic(programIndex,runningLineFloat);
            }
        }
            break;

        case MOVE_FREE://自由导航
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startMoveFree(programIndex,runningLineFloat);
            }
        }
            break;
     /**************************************************
     * 修改人：         hualei
     * 修改时间:  (1) 2018-09-17
     *            (2) 2018-09-30
     *            (3) 2018-10-04
     *
     * 修改说明:  (1) 增加匹配抓取 MUSIC 音乐指令实现接口
     *            (2) 增加匹配抓取 CHARGING 充电指令和 LIFT 升降机构指令实现接口
     *            (3) 增加匹配抓取 TCP 以太网通信指令实现接口
     *
     * ************************************************/
            //    MUSIC 音乐指令
        case MUSIC:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startMusic(programIndex,runningLineFloat);
            }
        }
            break;
            //   CHARGING 充电指令
        case CHARGING:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startCharging(programIndex,runningLineFloat);
            }
        }
            break;
            //   LIFT 升降机构指令
        case LIFT:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startLiftMechanism(programIndex,runningLineFloat);
            }
        }
            break;
        case TCP_START:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startTcpStart(programIndex,runningLineFloat);
            }
        }
            break;
        case TCP_READ:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startTcpRead(programIndex,runningLineFloat);
            }
        }
            break;
        case TCP_WRITE:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startTcpWrite(programIndex,runningLineFloat);
            }
        }
            break;
        case LOAD_MAP:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startLoadMap(programIndex,runningLineFloat);
            }
        }
            break;
        case CHECK_BATTERY:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startCheckBattery(programIndex,runningLineFloat);
            }
        }
            break;
        /**************************************************
        * 修改人：         丁培
        * 修改时间:  (1) 2018-11-30
        *
        * 修改说明:  (1) 添加呼叫指令
        *
        * ************************************************/
        case WAIT_COMAND:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startWaitCommand(programIndex,runningLineFloat);
            }
        }
        break;
        case SET_ODOM:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startSetOdom(programIndex,runningLineFloat);
            }
        }
        break;
        case ROBOT_MOVE:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startRobotMove(programIndex,runningLineFloat);
            }
        }
        break;
        case ROLLER:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startRoller(programIndex,runningLineFloat);
            }
        }
        break;
        case MODBUS_START:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startModbusStart(programIndex,runningLineFloat);
            }
        }
        break;
        case MODBUS_READ:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startModbusRead(programIndex,runningLineFloat);
            }
        }
        break;
        case MODBUS_WRITE:
        {
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startModbusWrite(programIndex,runningLineFloat);
            }
        }
        break;
        case INS_VISION_RUN:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startVisionRun(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_VISION_TRIGGER:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startVisionTrigger(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_VISION_GET_DATA:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startVisionGetData(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_VISION_CLEAR_DATA:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startVisionClearData(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_TRACK_START:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startTrackStart(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_TRACK_END:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startTrackEnd(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_TRACK_GET_DATA:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startTrackGetData(programIndex,runningLineFloat);
            }
             break;
        }
        //拓展新指令Fundation增加代码20191122hualei
        case INS_WELD_START:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startWeldStart(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_WELD_END:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startWeldEnd(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_WEAVE:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startWeave(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_WEAVE_END:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startWeaveEnd(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_TRACKWELD_TAST:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startTrackWeldTast(programIndex,runningLineFloat);
            }
             break;
        }

        case INS_TRACKWELD_END:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startTrackWeldEnd(programIndex,runningLineFloat);
            }
             break;
        }
        case INS_WATCHDI:
        {
            if((!isMainThread))
            {
                //添加报错消息处理，call，run 调用的文件中不能带有运动指令;
                cmdError(programIndex);
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3004, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            if(programIndex >= programSet->size())
            {
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                returnType = startWatchDI(programIndex,runningLineFloat);
            }
             break;
        }
        default:
        {
            //消息预留
            //指令错误不能识别
            addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3033, robotId ,
                   programInstructionTypeIn,runningLineFloat);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        }
    }

    return returnType;
}

int ProgramEngine::internalCease()
{
//    if(true==isMainThread)
//    {
//        qDebug()<<"internalCease";
//        programMcController->mcControllerInterpreter->allCease();
//    }
    engineStatus = ENUM_COMMAND_CEASE;

    for(int i = 0; i < programSet->size();i++)
    {
        updateProgramSetRunInformation(i);
    }

    oneStepEnableFlag=false;
    return 0;
}

int ProgramEngine::programEngineCease_noMotionThread()
{
    engineStatus = ENUM_COMMAND_CEASE;

    for(int i = 0; i < programSet->size();i++)
    {
        updateProgramSetRunInformation(i);
    }

    oneStepEnableFlag=false;
    return 0;
}

int ProgramEngine::engineStop()
{   
//    qDebug()<<"ProgramEngine::engineStop()"
//    <<"===,QThread::currentThreadId()="<<QThread::currentThreadId()
//    <<"===,currentThread->currentThreadId()="<<currentThread->currentThreadId()
//    <<"\n\n";
    engineStatus = ENUM_COMMAND_RESET;
    oneStepEnableFlag=false;



    if(isMainThread)
    {
        motionInstructionLogic->reset();
        programMcController->mcControllerInterpreter->setPalletFirstFlag(-1);

        programMcController->mcControllerInterpreter->allStop();

        #ifdef D_USE_ROS
        qDebug()<<"engineStop D_USE_ROS";
        musicInstructionLogic->stopMusic();
        liftMechanismInstructionLogic->stop();
        rollerInstructionLogic->stop();
#else
        weaveLogic->reset();
        #endif

        weldStartLogic->stopWeld();
        laserCameraInstructionLogic->reset();
        stopLaserScanBuffer();


    }
    waitCommandInstructionLogic->reset();
//    emit engineThreadDestory_signal();
//    engineThreadDestory();
    taskManager->resetTask();



    return 0;
}

int ProgramEngine::programSetReset()
{
//    programCountSize = 1;
    pcFlagValue = 0.0;
    runningLineFloat = 0.0;

    programSet->resize(1);
    (*programSet)[0].runningLineInt = 0;
    (*programSet)[0].runningLineInt+0.02;
    (*programSet)[0].timeOutFlag = false;
    if(true==isMainThread)
    {
        (*programSet)[0].runState = true;
    }
    (*programSet)[0].errorFlag=0;
    allPointProList->resize(1);
    if(allPointProList->size() > 0)
    {
        *currentPointProList = (*allPointProList)[0];
    }
    allVehicleDPointList.resize(1);
    if(allVehicleDPointList.size() > 0)
    {
        currentVehicleDPointList = allVehicleDPointList[0];
    }


    return 1;
}

int ProgramEngine::setRunModel(EN_CONTROL_TYPE runModelIn)
{
//   return taskManager->setRunModel(runModelIn);
    setRunModel_forChildTask(runModelIn);
    return setRunModel_taskManager(runModelIn,false);
}

int ProgramEngine::setRunModel_taskManager(EN_CONTROL_TYPE runModelIn,bool isManualLooseServoIn)
{
    if(runModelIn!=ENUM_CONTROL_AUTO && runModelIn!=ENUM_CONTROL_T1 &&
            runModelIn!=ENUM_CONTROL_T2 && runModelIn!=ENUM_CONTROL_EXT
            && runModelIn!=ENUM_CONTROL_FLEET )
    {
        addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine", 3028, robotId ,runModelIn);
        return -1;
    }
    if( engineStatus == ENUM_COMMAND_RESET  )
    {
        if(isMainThread)
        {
            programMcController->mcControllerInterpreter->resetAll();
        }

        if(runModelIn!=ENUM_CONTROL_FLEET && ENUM_CONTROL_FLEET==runModel )
        {
            addMsg(ENUM_MSG_REMIND, "ProgramEngine", "ProgramEngine", 3037, robotId ,runModelIn);
        }

         if((ENUM_CONTROL_EXT == runModelIn) ||(ENUM_CONTROL_AUTO == runModelIn)
                 ||(ENUM_CONTROL_FLEET == runModelIn))
         {
             if(ENUM_CONTROL_FLEET==runModelIn)
             {
                 if(NULL!=fleetService)
                 {
                     fleetService->clearFleetCommandQueue();
                 }

             }
            runModel=runModelIn;
            if(isMainThread)
            {
//                programMcController->mcControllerInterpreter->servoOn();
                programMcController->mcControllerInterpreter->controlTypeSwitch(runModelIn);
                liftMechanismInstructionLogic->setJogButtonEnable(false);
            }

         }
         else if((ENUM_CONTROL_T1 == runModelIn) ||(ENUM_CONTROL_T2 == runModelIn))
         {
             runModel=runModelIn;
             if(isMainThread)
             {
                 if(isManualLooseServoIn)
                 {
                        programMcController->mcControllerInterpreter->servoOff();
                 }
                 programMcController->mcControllerInterpreter->controlTypeSwitch(runModelIn);
                 liftMechanismInstructionLogic->setJogButtonEnable(true);
             }
         }
         else
         {
             return -1;
         }

    }
    else
    {
        addMsg(ENUM_MSG_REMIND, "ProgramEngine", "ProgramEngine", 3038, robotId ,runModelIn);
        return -1;
    }

    return 1;
}

int ProgramEngine::getProgramEngineStatus()
{
    return engineStatus;
}

int ProgramEngine::getCurrentRobotRunFile(CurrentRunFile &runRobotFile)
{
    return taskManager->getCurrentRobotRunFile(runRobotFile);

}

int ProgramEngine::getProgramSetRunFile(QString &fileName)
{
    if(programSet->size()>0)
    {
        fileName=(*programSet)[0].thisProgramName;
        return 1;
    }
    return -1;
}

int ProgramEngine::updateControllerInfoLevel2( UpdateDataLv2  &updateDataLv2 )
{
      //寄存器模块
//     updateDataLv2.localRegister = regData->regRValue.toStdVector();
//     updateDataLv2.pointRegister = regData->regPrValue.toStdVector();
//     updateDataLv2.plRegister = programSet->regPlList.toStdVector();


//     int runStatus = programMcController->mcControllerInterpreter->getStatus();
     int runStatus = engineStatus;

     //不要用trylock
//     if(trylock())
//     {
//         qDebug()<<"===mcControllerInterpreter===="<< runStatus<<"==engineStatus===="<<engineStatus<<"\n\n";
     lock();
     if(ENUM_RUN == runStatus)
     {
         runStatus = ENUM_COMMAND_RUN;
     }
     else if(ENUM_CEASE < runStatus)
     {
         runStatus = ENUM_COMMAND_RESET;
         //engineStatus = ENUM_COMMAND_RESET; hualei delete
     }
     else
     {
             runStatus = engineStatus;
     }
     //状态
     updateDataLv2.globalCurrentState = runStatus;
     updateDataLv2.globalHandState = runModel;
     updateDataLv2.globalHandRunStatus = moveType;
     updateDataLv2.startType = startType;
     updateDataLv2.currentRobotID = robotId;
     updateDataLv2.mainProgramFileVersion=programFileVersionInfo;

//         qDebug()<<"===runStatus===="<< runStatus<<"==runModel===="<<runModel<<"\n\n";
    unlock();
//     }

     //当前正在运行的程序文件
     getCurrentRobotRunFile( updateDataLv2.currentRunFile );

     return 1;
}

int ProgramEngine::updateControllerInfoLevel3(UpdateDataLv3 &updateDataLv3)
{
    if(   NULL == regData   )
    {
        qDebug() << "****lv2********program*******************************this is empty!!!!!!!!!!!\n\n"
                << regData
                 <<endl;
        return 0;
    }
    regData->updateControllerInfoLevel3( updateDataLv3 );



//    if(trylock())
//    {
//        QVector <float > tmpTimer;
//        tmpTimer.resize(TIMER_SIZE);
//    //    tmpTimer = regData->regTimerValue;

//        for(int i = 0; i < tmpTimer.size();i++)
//        {
//            if(TIMER_PROCESS_STOP != programUpdateDataList.timerProcessType[i])
//            {
//                getCurrentUpdateTime(i);
//            }
//        }
//        tmpTimer = programUpdateDataList.timerCount;

//    //    qDebug() << __FUNCTION__<<"====tmp===="<<tmpTimer<<"\n\n\n\n\n";

//        updateDataLv3.timerRegister = tmpTimer.toStdVector();
//        unlock();
//     }

    return 0;
}

double ProgramEngine::countIntervalTime(timeval startTimeVal, timeval endTimeVal)
{
    float timeInterval = 0.0;

    float tmp1 = endTimeVal.tv_sec - startTimeVal.tv_sec;
    float tmp2 = (endTimeVal.tv_usec - startTimeVal.tv_usec)/MICROSECOND_TIME;

    timeInterval = tmp1 + tmp2;

    return timeInterval;
}



void ProgramEngine::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4,
                           QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {
    case 3043:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("用户程序格式错误！");
        break;
    }
    case 3042:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("run指令错误，子程序内无指令！");
        break;
    }
    case 3041:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("call指令错误，子程序内无指令！");
        break;
    }
    case 3040:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("寄存器号(%1)超范围！").arg(parameter1);
        break;
    }
    case 3039:
    {
        infomationStr = QObject::tr("机器人:") +
                QString::number(robotId) +
                QObject::tr("非停止状态，禁止设置运行行号！");
         break;
    }
    case 3038:
    {
        infomationStr = QObject::tr("机器人:") +
                QString::number(robotId) +
                QObject::tr("请点停止按钮后，再切换控制模式！");
         break;
    }
    case 3037:
    {
        infomationStr = QObject::tr("机器人:") +
                QString::number(robotId) +
                QObject::tr("已退出调度模式！");
         break;
    }
    case 3036:
    {
        infomationStr = QObject::tr("机器人:") +
                QString::number(robotId) +
                QObject::tr("无法设置，没有打开包含码垛程序的文件！");
         break;
    }
    case 3035:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("请进入停止状态，再重新打开程序！");
        break;
    }
    case 3034:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("当前处于调度模式，启动运行状态！");
        break;
    }
    case 3033:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("当前行指令类型不支持,指令类别码=%1, 行号%2！").arg(parameter1).arg(parameter2);
        break;
    }
    case 3032:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr +
                QObject::tr("; 程序(%2)指令(行号%1)运行出错！").arg(parameter1)
                .arg(message);
        break;

    }
    case 3031:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",程序(%1)运行完结束指令，任务停止！").arg(message);
        break;
    }
    case 3030:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",子程序(%1)不允许调用run指令，请修改程序！").arg(message);
        break;
    }
    case 3029:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",程序(%1)运行完最后一行，没有指令可以运行！").arg(message);
        break;
    }
    case 3028:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("控制模式runMode范围不合法，切换失败,runMode=%1！").arg(parameter1);
        break;
    }

    case 3027:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("程序指令错误,停止运行！");


    }
      break;

    case 3026:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("用户主程式执行完毕！！");


    }
      break;
    case 3025:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("ProgramEngine初始化失败，无法运行程序！");


    }
        break;
    case 3002:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",程序执行完毕！");
    }
        break;

    case 3004:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; 非主任务，不能运行运动类指令");

    }
        break;

    case 3006:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; 程序文件格式错误，导致取点不正确! 运行停止！");
    }
        break;
    case 3007:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; 无此点位数据！")  + QString::number(parameter1);
    }
        break;
    case 3008:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",程式名为空! ");

    }
        break;
    case 3009:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId) + QObject::tr(",") ;
        infomationStr = infomationStr + message + QObject::tr("程式文件不存在!");

    }
        break;
    case 3010:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",读取文件指令错误,行号") + QString::number(parameter1);
    }
        break;
    case 3021:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; run 调用的文件中不能带有PL指令");

    }
        break;
    case 3022:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; run调用文件不能超过9个!!!");
        break;

    }

    case 3023:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（算法模块进入停止状态）！");
       break;
    }



    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}


